A method for obstacle avoidance trajectory planning for nuclear facility decommissioning robots

Abstract

This invention discloses a method for obstacle avoidance trajectory planning for nuclear facility decommissioning robots. The method collects environmental data using solid-state LiDAR and a binocular camera, and establishes a 3D point cloud map based on 3D mapping and localization algorithms. Using the robot's front binocular camera and the SGBM algorithm, the distance from obstacles to the robot's current centroid is determined. The robot is then transformed into a global Cartesian coordinate system, and the 2D plane coordinates of all point clouds are extracted. Obstacle point clouds within the boundaries are selected using two linear equations. An effective obstacle boundary curve is generated for the current moment. Local candidate trajectories for the robot are generated through point sampling and fifth-order polynomial trajectory fitting. The effective obstacle boundary is dynamically solved at each moment, thus planning the optimal trajectory for the current moment until the robot avoids the obstacle. This method solves the problem of determining the distance between the robot and its surrounding environment when nuclear decommissioning robots currently rely on remote human operation via monitoring video during obstacle avoidance. It not only reduces the burden of manual operation but also improves the efficiency and accuracy of obstacle avoidance.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, and more specifically, to a method for planning obstacle avoidance trajectories for a nuclear facility decommissioning robot. Background Technology

[0002] Due to its high radioactivity, long half-life, and high biotoxicity, the treatment and disposal of nuclear waste has become a global concern. Currently, the industry's process for treating high-level radioactive waste is "vitrification-geological disposal," with high-level radioactive waste storage and transportation containers serving as the first line of defense. The manufacture of high-level radioactive waste storage and transportation containers can fill a gap in the industry and is of great significance to the development of nuclear waste reprocessing engineering.

[0003] Currently, in the field of nuclear facility decommissioning, robot motion control is typically achieved through manual remote control. However, due to the complex environmental structure of nuclear facility decommissioning scenarios, operators cannot accurately perceive the distance to surrounding obstacles by relying solely on images transmitted from cameras, especially in confined work spaces. This significantly increases the difficulty of operation, reduces work efficiency, and easily leads to operational errors that could damage the robot. Summary of the Invention

[0004] The purpose of this invention is to provide a method for obstacle avoidance trajectory planning for nuclear facility decommissioning robots, which aims to improve the safety and efficiency of nuclear facility decommissioning operations.

[0005] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0006] A method for obstacle avoidance trajectory planning for a nuclear facility decommissioning robot includes the following steps:

[0007] S1 collects environmental data through solid-state lidar and binocular cameras, and builds a 3D point cloud map based on 3D mapping and positioning algorithms;

[0008] S2, using the front-facing binocular camera based on the SGBM algorithm, determines the distance from the obstacle to the robot's current center of mass;

[0009] S3. With the rear axis center of the robot as the origin, establish a Cartesian coordinate system with the robot's motion direction as the longitudinal axis and the x-axis as the y-axis. Based on the Cartesian coordinate system, calculate the coordinates of the robot's four vertices in the clockwise direction and transform them to the global Cartesian coordinate system. Extract the two-dimensional plane coordinates of all point clouds. Using two linear equations as boundaries, filter out the obstacle point clouds within the boundaries.

[0010] S4, calculate the longitudinal distance from all obstacle point clouds to the robot, and retain the obstacle point cloud with the closest longitudinal distance to the preset number of samples based on the preset number of obstacle point cloud samples, as the set of effective obstacle boundary point clouds at this time; then fit the effective boundary point cloud based on the cubic spline curve to generate the effective obstacle boundary curve at the current time.

[0011] S5, based on the Lattice Planner trajectory planning algorithm, generates local candidate trajectories for the robot through point sampling and fifth-order polynomial trajectory fitting;

[0012] S6: Determine the relationship between each candidate trajectory and the current effective boundary curve of the obstacle. If they intersect, delete them; otherwise, retain them.

[0013] S7. Based on the principle of minimum cost, compare all candidate trajectories that do not intersect with the current effective boundary curve of the obstacle to determine the local optimal motion trajectory.

[0014] S8 publishes the path point pose information of the motion trajectory to the control layer, transmits the control signal to the servo motor driver, controls the hydraulic oil pump to work, and then makes the hydraulic motor work.

[0015] S9. At any moment when the robot is avoiding obstacles, steps S3-S8 are executed repeatedly to dynamically solve the effective boundary of the obstacle at each moment, thereby planning the best motion trajectory at the current moment until the robot bypasses the obstacle.

[0016] Furthermore, in step S2, the formula for calculating the distance from the obstacle to the robot's current center of mass is as follows:

[0017] dis(x) = argmin(C(x,d))

[0018] =D(x,d)+P1*min(C(x-1,d),C(x,d-1))+P2*min(C(x

[0019] -1,d-1),C(x-1,d+1))

[0020] distance(x)=baseline*f / dis(x)

[0021] In the formula, dis(x) represents the disparity value of the target point x, argmin represents the value of d that minimizes C(x,d), C(x,d) represents the cost of the target point (x,d) table, D(x,d) represents the disparity value of the target point (x,d), P1 and P2 are the weights of the smoothing term; distance(x) represents the distance of the target point x, baseline represents the baseline length of the stereo camera, and f represents the focal length of the stereo camera.

[0022] Furthermore, in step S3, the transformation relationship from its own Cartesian coordinate system to the global Cartesian coordinate system is as follows:

[0023]

[0024] In the formula, This represents the robot's position in global Cartesian coordinates. Let represent the vertex coordinates of the robot in its own Cartesian coordinate system, where i = 1, 2, 3, 4; θ represents the robot's heading angle. This represents the vertex coordinates of the robot in the global Cartesian coordinate system; where, in the global Cartesian coordinate system, the vertices are respectively... and apex By performing a linear fit, we obtain two straight line equations:

[0025]

[0026] R represents any real number, and j represents the linear equation number j = 1, 2.

[0027] Further, in step S4, the generated effective obstacle boundary curve for the current moment is:

[0028] S τi (t)=a i +b i (tt i )+c i (tt i ) 2 +d i (tt i ) 3

[0029] In the formula, a i ,b i ,c i ,d i S represents the spline coefficient of the i-th segment of the curve. τi (t) represents the i-th curve segment at time t in the τ direction, where τ represents the x-direction or y-direction.

[0030] Further, in step S5, the generated local candidate trajectory for the robot is:

[0031]

[0032] In the formula: a0, a1, a2, a3, a4, ..., a5 represent the coefficients of the fifth-degree polynomial, and t is the time parameter.

[0033] Compared with the prior art, the present invention has the following beneficial effects:

[0034] (1) This invention improves the intelligence level of nuclear decommissioning robots and solves the problem that the distance between the robot and the surrounding environment is difficult to determine when the nuclear decommissioning robot needs to be remotely operated by human through monitoring video during obstacle avoidance. This not only reduces the burden of human operation, but also improves the efficiency and accuracy of obstacle avoidance.

[0035] (2) This invention proposes a dynamic obstacle boundary generation method, which improves the problem of inaccurate description of obstacle boundaries in traditional obstacle avoidance methods (often larger than the actual obstacle boundary). It can describe obstacle boundaries more accurately, improve obstacle avoidance performance, and better adapt to the complex and confined working environment of nuclear decommissioning. Attached Figure Description

[0036] Figure 1 Structural block diagram of the nuclear decommissioning robot control system.

[0037] Figure 2 A schematic diagram of the dynamic generation of obstacle boundaries and trajectory planning for a nuclear decommissioning robot.

[0038] Figure 3 Flowchart of the control program for nuclear decommissioning robots. Detailed Implementation

[0039] The present invention will be further described below with reference to the accompanying drawings and embodiments. The embodiments of the present invention include, but are not limited to, the following embodiments.

[0040] Example

[0041] like Figures 1-3 As shown, this invention discloses a method for obstacle avoidance trajectory planning for a nuclear facility decommissioning robot. This method is based on a robot obstacle avoidance system, which consists of three main modules: a perception module, a computing platform, and a control module. The program is based on a distributed design using the ROS system and is integrated into the computing platform. The main functions of each module are as follows:

[0042] The perception module consists of a solid-state lidar (MID-360) and a binocular camera (MYNT EYE D1010-IR-50 / COLOR). Its main function is to collect environmental data around the robot and transmit it to the computing platform as initial parameters for the program.

[0043] The computing platform consists of a processor (NVIDIA Jetson Xavier NX 16GB module) and an Ubuntu 18.04LTS / ROS system. Its main functions are to execute algorithm programs and issue control signals.

[0044] Control module: Composed of CAN bus and servo motor driver, its main function is to transmit control signals to the servo motor driver to control the hydraulic pump, which in turn drives the hydraulic motor.

[0045] The implementation steps of this method are as follows:

[0046] Step 1: Collect environmental data using solid-state LiDAR and binocular cameras, and build a 3D point cloud map based on 3D mapping and localization algorithms;

[0047] Step 2: Using the front-facing binocular camera and the SGBM algorithm, determine the distance from the obstacle to the robot's current center of mass; the formula is as follows:

[0048] dis(x) = argmin(C(x,d))

[0049] =D(x,d)+P1*min(C(x-1,d),C(x,d-1))+P2*min(C(x

[0050] -1,d-1),C(x-1,d+1))

[0051] distance(x)=baseline*f / dis(x)

[0052] In the formula, dis(x) represents the disparity value of the target point x, argmin represents the value of d that minimizes C(x,d), C(x,d) represents the cost of the target point (x,d) table, D(x,d) represents the disparity value of the target point (x,d), P1 and P2 are the weights of the smoothing term; distance(x) represents the distance of the target point x, baseline represents the baseline length of the stereo camera, and f represents the focal length of the stereo camera.

[0053] In the Cartesian coordinate system of the vehicle body, the position coordinates (x, y, y) of the obstacle are determined using distance and vehicle heading angle θ. obs y obs The coordinates are transformed into a global Cartesian coordinate system using a coordinate transformation matrix. Using this coordinate system as the center of the obstacle, an adaptive density clustering algorithm is used to filter out the obstacle point cloud information.

[0054] Step 3: Whether the robot collides with an obstacle depends on the robot's current direction of motion (direction of the robot's front) and the obstacle range defined by the robot's maximum width (vehicle width). (At any given moment, only a localized collision with the obstacle is possible, not the entire obstacle). Based on this principle, a Cartesian coordinate system is established with the robot's rear axis center as the origin, with the x-axis (robot's direction of motion) and the y-axis. Given the robot's length and width, the coordinates of the robot's four vertices are calculated clockwise using this Cartesian coordinate system. Then transform to the global Cartesian coordinate system. The transformation relationship between the coordinate systems is as follows:

[0055]

[0056] In the formula, This represents the robot's position in global Cartesian coordinates. Let represent the vertex coordinates of the robot in its own Cartesian coordinate system, where i = 1, 2, 3, 4; θ represents the robot's heading angle. This represents the vertex coordinates of the robot in the global Cartesian coordinate system.

[0057] In the global Cartesian coordinate system, respectively, the vertices and apex By performing a linear fit, we obtain two straight line equations:

[0058]

[0059] R represents any real number, and j represents the linear equation number j = 1, 2. Extract the two-dimensional planar coordinates of all obstacle point clouds. In the global Cartesian coordinate system, the obstacle point cloud within the boundary is selected using two linear equations as the boundary.

[0060] Step 4: Calculate the longitudinal distance from the point cloud of all obstacles to the robot. Based on a preset number of point cloud samples W (if the actual number of point clouds is less than W, then W = W - 1, until it is not less than the actual number of point clouds), the W point clouds with the closest vertical distance are retained as the set of effective obstacle boundary point clouds at this time. Then, the effective boundary point clouds are fitted based on cubic spline curves to generate the effective obstacle boundary curve at the current moment:

[0061] S τi (t)=a i +b i (tt i )+c i (tt i ) 2 +d i (tt i ) 3

[0062] In the formula, a i ,b i ,c i ,d i S represents the spline coefficient of the i-th segment of the curve. τi (t) represents the i-th curve segment at time t in the τ direction, where τ represents the x-direction or y-direction.

[0063] Step 5: Based on the Lattice Planner trajectory planning algorithm, generate local candidate trajectories for the robot through point sampling and fifth-order polynomial trajectory fitting:

[0064]

[0065] In the formula: a0, a1, a2, a3, a4, ..., a5 represent the coefficients of the fifth-degree polynomial, and t is the time parameter.

[0066] Step 6: Determine the relationship between each candidate trajectory and the current effective boundary curve of the obstacle. If they intersect, delete them; otherwise, keep them.

[0067] Step 7: Based on the principle of minimizing cost, compare all candidate trajectories that do not intersect with the current effective boundary curve of the obstacle to determine the local optimal motion trajectory.

[0068] Step 8: Publish the path point pose information of the motion trajectory to the control layer. Based on the PID algorithm, transmit the control signal to the servo motor driver through CAN bus communication to control the hydraulic oil pump to work, thereby making the hydraulic motor work.

[0069] Step 9: At any moment during the robot's obstacle avoidance, repeat steps 3 to 8 to dynamically solve for the effective boundary of the obstacle at each moment, thereby planning the optimal motion trajectory for the current moment until the robot bypasses the obstacle.

[0070] Through the above design, this invention improves the intelligence level of nuclear decommissioning robots and solves the problem that it is difficult to determine the distance between the robot and the surrounding environment when nuclear decommissioning robots rely on remote operation via monitoring video during obstacle avoidance. This not only reduces the burden of manual operation but also improves the efficiency and accuracy of obstacle avoidance.

[0071] The above embodiments are merely one of the preferred embodiments of the present invention and should not be used to limit the scope of protection of the present invention. Any modifications or refinements made to the main design concept and spirit of the present invention that are not of substantial significance, but solve the same technical problem as the present invention, should be included within the scope of protection of the present invention.

Claims

1. A method for obstacle avoidance trajectory planning for a nuclear facility decommissioning robot, characterized in that, Includes the following steps: S1 collects environmental data through solid-state lidar and binocular cameras, and builds a 3D point cloud map based on 3D mapping and positioning algorithms; S2, using the front-facing binocular camera based on the SGBM algorithm, determines the distance from the obstacle to the robot's current center of mass; S3. With the rear axis center of the robot as the origin, establish a Cartesian coordinate system with the robot's motion direction as the longitudinal axis and the x-axis as the y-axis. Based on the Cartesian coordinate system, calculate the coordinates of the robot's four vertices in the clockwise direction and transform them to the global Cartesian coordinate system. Extract the two-dimensional plane coordinates of all point clouds. Using two linear equations as boundaries, filter out the obstacle point clouds within the boundaries. S4, calculate the longitudinal distance from all obstacle point clouds to the robot, and retain the obstacle point cloud with the closest longitudinal distance to the preset number of samples based on the preset number of obstacle point cloud samples, as the set of effective obstacle boundary point clouds at this time; then fit the effective boundary point cloud based on the cubic spline curve to generate the effective obstacle boundary curve at the current time. S5, based on the Lattice Planner trajectory planning algorithm, generates local candidate trajectories for the robot through point sampling and fifth-order polynomial trajectory fitting; S6: Determine the relationship between each candidate trajectory and the current effective boundary curve of the obstacle. If they intersect, delete them; otherwise, retain them. S7. Based on the principle of minimum cost, compare all candidate trajectories that do not intersect with the current effective boundary curve of the obstacle to determine the local optimal motion trajectory. S8 publishes the path point pose information of the motion trajectory to the control layer, transmits the control signal to the servo motor driver, controls the hydraulic oil pump to work, and then makes the hydraulic motor work. S9. At any moment when the robot is avoiding obstacles, steps S3-S8 are executed repeatedly to dynamically solve the effective boundary of the obstacle at each moment, thereby planning the best motion trajectory at the current moment until the robot bypasses the obstacle.

2. The obstacle avoidance trajectory planning method for a nuclear facility decommissioning robot according to claim 1, characterized in that, In step S2, the formula for calculating the distance from the obstacle to the robot's current center of mass is as follows: dis(x)=argmin(C(x,d)) =D(x,d)+P1*min(C(x-1,d),C(x,d-1))+P2*min(C(x-1,d-1),C(x-1,d+1)) distance(x)=baseline*f / dis(x) In the formula, dis(x) represents the disparity value of the target point x, argmin represents the value of d that minimizes C(x,d), C(x,d) represents the cost of the target point (x,d) table, D(x,d) represents the disparity value of the target point (x,d), P1 and P2 are the weights of the smoothing term; distance(x) represents the distance of the target point x, baseline represents the baseline length of the stereo camera, and f represents the focal length of the stereo camera.

3. The obstacle avoidance trajectory planning method for a nuclear facility decommissioning robot according to claim 2, characterized in that, In step S3, the transformation relationship from its own Cartesian coordinate system to the global Cartesian coordinate system is as follows: In the formula, This represents the robot's position in global Cartesian coordinates. Let represent the vertex coordinates of the robot in its own Cartesian coordinate system, where i = 1, 2, 3, 4; θ represents the robot's heading angle. This represents the vertex coordinates of the robot in the global Cartesian coordinate system; where, in the global Cartesian coordinate system, the vertices are respectively... and apex By performing a linear fit, we obtain two straight line equations: R represents any real number, and j represents the linear equation number j = 1, 2.

4. The obstacle avoidance trajectory planning method for a nuclear facility decommissioning robot according to claim 3, characterized in that, In step S4, the generated effective obstacle boundary curve for the current moment is: S τi (t)=a i +b i (t-t i )+c i (t-t i ) 2 +d i (t-t i ) 3 In the formula, a i ,b i ,c i ,d i S represents the spline coefficient of the i-th segment of the curve. τi (t) represents the i-th curve segment at time t in the τ direction, where τ represents the x-direction or y-direction.

5. The obstacle avoidance trajectory planning method for a nuclear facility decommissioning robot according to claim 4, characterized in that, In step S5, the generated local candidate trajectory for the robot is as follows: In the formula: a0, a1, a2, a3, a4, a5 represent the coefficients of the fifth-degree polynomial, and t is the time parameter.

Images