Nuclear facility decommissioning robot obstacle avoidance trajectory planning method
Data is collected through solid-state lidar and binocular cameras, combined with three-dimensional map construction and trajectory planning algorithms, and dynamically generates robot obstacle avoidance paths, solving the problem that artificial remote control can hardly perceive obstacle distances during the retirement of nuclear facilities, and improving the efficiency of robot obstacle avoidance and accuracy.
Patent Information
- Application Number
- CN202410972494.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-19
- Publication Date
- 2024-11-08
- Estimated Expiration
- 2044-07-19
AI Technical Summary
During the decommissioning of nuclear facilities, artificial remote remote control robots have difficulty accurately sensing the distance of surrounding obstacles, especially in small spaces, resulting in increased operational difficulty, reduced efficiency and increased risk of robot damage.
Solid-state lidar and binocular cameras are used to collect environmental data, and point cloud maps are established through three-dimensional mapping and positioning algorithms. The distance between obstacles is determined by SGBM algorithm, and the obstacle boundaries are fitted using cubic splines, and combined with Lattice Planner trajectory planning algorithm to generate Local candidate trajectories, and the optimal motion trajectory is determined through the principle of minimum cost, and the robot's obstacle avoidance path is dynamically planned.
It improves the safety and efficiency of nuclear facilities decommissioning operations, reduces the burden of manual manipulation, and enhances the robot's obstacle avoidance accuracy and adaptability in complex environments.
Smart Images

Figure CN118915748A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of robot control technology, and in particular to a method for planning obstacle avoidance trajectories of a nuclear facility decommissioning robot. Background Art
[0002] Nuclear waste has the characteristics of high radiation level, long half-life period and high biological toxicity, so its treatment and disposal has become a worldwide concern. At present, the process route adopted by the industry for high-level nuclear waste treatment is "vitrification-geological disposal", and high-level nuclear waste storage and transportation containers are the first safety barrier. The manufacture of high-level nuclear waste storage and transportation containers can fill the gap in the industry and is of great significance to the development of nuclear waste reprocessing projects in the industry.
[0003] Currently, in the field of nuclear facility decommissioning, manual remote control is usually used to control robot motion. However, due to the complex environmental structure of nuclear facility decommissioning scenes, operators can hardly accurately perceive the distance of surrounding obstacles by relying only on the images sent back by the camera, especially in a small working space. This significantly increases the difficulty of operation, reduces the operating efficiency, and easily causes damage to the robot due to operational errors. Summary of the invention
[0004] The purpose of the present invention is to provide a nuclear facility decommissioning robot obstacle avoidance trajectory planning method, aiming to improve the safety and efficiency of nuclear facility decommissioning operations.
[0005] To achieve the above purpose, the technical solution adopted by the present invention is as follows:
[0006] A method for obstacle avoidance trajectory planning of a nuclear facility decommissioning robot comprises the following steps:
[0007] S1, collects environmental data through solid-state laser radar and binocular camera, and builds a 3D point cloud map based on 3D mapping and positioning algorithms;
[0008] S2, using the robot’s front binocular camera based on the SGBM algorithm, determines the distance from the obstacle to the robot’s current center of mass;
[0009] S3, taking the center of the rear axle of the robot as the origin, establish the robot's own Cartesian coordinate system with the robot's motion direction as the longitudinal direction and the x-axis as the y-axis. Based on the robot's own Cartesian coordinate system, calculate the coordinates of the four vertices of the robot in the clockwise direction, and convert them to the global Cartesian coordinate system. Extract the two-dimensional plane coordinates of all point clouds, and use the two linear equations as boundaries to filter out the obstacle point clouds within the boundaries.
[0010] S4, calculating the longitudinal distances from all obstacle point clouds to the robot, retaining the obstacle point clouds with the preset number of samplings of the closest longitudinal distances as the obstacle effective boundary point cloud set at this time according to the preset number of obstacle point cloud samplings; then fitting the effective boundary point clouds based on the cubic spline curve to generate the obstacle effective boundary curve at the current moment;
[0011] S5, based on the Lattice Planner trajectory planning algorithm, generates the robot's local candidate trajectory through scattered point sampling and quintic polynomial trajectory fitting;
[0012] S6, by judging the relationship between each candidate trajectory and the effective boundary curve of the current obstacle, if there is an intersection, it is deleted, otherwise, it is retained;
[0013] S7, based on the principle of minimum cost, compare all candidate trajectories that do not intersect with the effective boundary curve of the current obstacle to determine the local optimal motion trajectory;
[0014] S8, publishing the path point position information of the motion trajectory to the control layer, transmitting the control signal to the servo motor driver, controlling the hydraulic oil pump to work, and then making the hydraulic motor work;
[0015] S9, at any time 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 distance from the obstacle to the current center of mass of the robot is calculated 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-1,d-1),C(x-1,d+1))
[0020] distance(x)=baseline*f / dis(x)
[0021] Where 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 the local Cartesian coordinate system to the global Cartesian coordinate system is:
[0023]
[0024] In the formula, represents the position of the robot in global Cartesian coordinates, represents the vertex coordinates of the robot in its own Cartesian coordinate system, where i = 1, 2, 3, 4; θ represents the robot's heading angle, represents the vertex coordinates of the robot in the global Cartesian coordinate system; in the global Cartesian coordinate system, the vertices and vertices Perform linear fitting and obtain two straight line equations:
[0025]
[0026] R represents an arbitrary real number, and j represents the linear equation number j=1,2.
[0027] Furthermore, in step S4, the generated obstacle effective boundary curve at 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 represents the spline coefficient of the i-th segment curve, S τi (t) represents the i-th curve corresponding to time t in the τ direction, and τ represents the x direction or the y direction.
[0030] Furthermore, in step S5, the generated local candidate trajectory of the robot is:
[0031]
[0032] In the formula: a0, a1, a2, a3, a4, and a5 represent the coefficients of the quintic polynomial, and t is the time parameter.
[0033] Compared with the prior art, the present invention has the following beneficial effects:
[0034] (1) The present invention improves the intelligence level of the nuclear decommissioning robot and solves the problem that the distance between the robot and the surrounding environment is difficult to determine when the nuclear decommissioning robot currently relies on manual remote control through monitoring video during obstacle avoidance. This not only reduces the burden of manual operation, but also improves the efficiency and accuracy of obstacle avoidance.
[0035] (2) The present invention proposes a method for dynamically generating obstacle boundaries, which improves the problem of inaccurate description of obstacle boundaries (often larger than the actual obstacle boundaries) in traditional obstacle avoidance methods. It can more accurately describe obstacle boundaries, improve obstacle avoidance performance, and better adapt to the complex and narrow working environment of nuclear decommissioning. BRIEF DESCRIPTION OF THE DRAWINGS
[0036] Figure 1 Block diagram of the control system for the nuclear decommissioning robot.
[0037] Figure 2 Schematic diagram of dynamic generation of obstacle boundaries and trajectory planning for the nuclear decommissioning robot.
[0038] Figure 3 Workflow diagram of the nuclear decommissioning robot control program. DETAILED DESCRIPTION
[0039] The present invention is further described below in conjunction with 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 to 3 As shown, the present invention discloses a nuclear facility decommissioning robot obstacle avoidance trajectory planning method, which is implemented based on the robot obstacle avoidance system. The entire system consists of three main modules: perception module, computing platform, and control module. The program is based on the ROS system distributed design. The entire program is integrated in the computing platform. The main functions of each module are as follows:
[0042] Perception module: It consists of a solid-state laser radar (MID-360) and a binocular camera (MYNT EYE D1010-IR-50 / COLOR). Its main function is to collect data about the robot's surrounding environment and transmit it to the computing platform as the initial parameters of the program.
[0043] Computing platform: It consists of a processor (NVIDIA Jetson Xavier NX 16GB module) and deploys the Ubuntu18.04LTS / ROS system. Its main function is to execute algorithm programs and issue control signals.
[0044] Control module: It consists of CAN bus and servo motor driver. Its main function is to transmit control signals to the servo motor driver to control the operation of the hydraulic pump, thereby driving the hydraulic motor to work.
[0045] The steps to implement this method are as follows:
[0046] Step 1: Collect environmental data through solid-state laser radar and binocular camera, and build a 3D point cloud map based on 3D mapping and positioning algorithms;
[0047] Step 2: Determine the distance from the obstacle to the current center of mass of the robot using the SGBM algorithm using the robot's front binocular camera. 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] Where 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 vehicle body Cartesian coordinate system, the obstacle position coordinates (x obs ,y obs ), transformed into the global Cartesian coordinate system through the coordinate transformation matrix, taking this as the obstacle circle center, and filtering out the obstacle point cloud information through the adaptive density clustering algorithm.
[0054] Step 3: Whether the robot collides with an obstacle depends on the direction of the robot's movement (the direction of the front of the vehicle) at the current moment, and is related to the obstacle interval corresponding to the robot's maximum width (body width) as the boundary (at any moment, only a local position of the obstacle may collide, not the entire obstacle). Based on the above principle, take the center of the robot's rear axle as the origin, establish its own Cartesian coordinate system with the longitudinal direction (robot movement direction) as the x-axis and the horizontal direction as the y-axis. When the length and width of the robot are known, calculate the coordinates of the four vertices of the robot in a clockwise direction based on its own Cartesian coordinate system. And converted to the global Cartesian coordinate system, the transformation relationship between coordinate systems is:
[0055]
[0056] In the formula, represents the position of the robot in global Cartesian coordinates, represents the vertex coordinates of the robot in its own Cartesian coordinate system, where i = 1, 2, 3, 4; θ represents the robot's heading angle, Represents the vertex coordinates of the robot corresponding to the global Cartesian coordinate system.
[0057] In the global Cartesian coordinate system, the vertices and vertices Perform linear fitting and obtain two straight line equations:
[0058]
[0059] R represents any real number, j represents the linear equation number j=1,2. Extract the two-dimensional plane coordinates of all obstacle point clouds In the global Cartesian coordinate system, two linear equations are used as boundaries to filter out the obstacle point cloud within the boundaries.
[0060] Step 4: Calculate the longitudinal distance from all obstacle point clouds to the robot According to the preset number of point cloud sampling 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), retain the W point clouds with the closest longitudinal distance as the effective boundary point cloud set of the obstacle at this time. Then fit the effective boundary point cloud based on the cubic spline curve to generate the effective boundary curve of the obstacle 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 represents the spline coefficient of the i-th segment curve, S τi (t) represents the i-th curve corresponding to time t in the τ direction, and τ represents the x direction or the y direction.
[0063] Step 5: Based on the Lattice Planner trajectory planning algorithm, generate the robot's local candidate trajectory through scattered point sampling and quintic polynomial trajectory fitting:
[0064]
[0065] In the formula: a0, a1, a2, a3, a4, and a5 represent the coefficients of the quintic polynomial, and t is the time parameter.
[0066] Step 6: Determine the relationship between each candidate trajectory and the effective boundary curve of the current obstacle. If there is an intersection, delete it; otherwise, keep it.
[0067] Step 7: Based on the principle of minimum cost, compare all candidate trajectories that do not intersect with the effective boundary curve of the current obstacle to determine the local optimal motion trajectory.
[0068] Step 8: Publish the path point position information of the motion trajectory to the control layer, and transmit the control signal to the servo motor driver through CAN bus communication based on the PID algorithm to control the operation of the hydraulic oil pump and then make the hydraulic motor work.
[0069] Step 9: At any time when the robot is avoiding obstacles, loop through steps 3 to 8 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.
[0070] Through the above design, the present invention improves the intelligence level of the nuclear decommissioning robot, solves the problem that the distance between the robot and the surrounding environment is difficult to determine when the current nuclear decommissioning robot needs to rely on manual remote operation through monitoring video during the obstacle avoidance process. It not only reduces the burden of manual operation, but also improves the efficiency and accuracy of obstacle avoidance.
[0071] The above embodiment is only one of the preferred implementation modes of the present invention and should not be used to limit the protection scope of the present invention. Any changes or modifications that are made to the main design concept and spirit of the present invention and have no substantive significance, and the technical problems they solve are still consistent with the present invention, should be included in the protection scope of the present invention.
Claims
1. A nuclear facility decommissioning robot obstacle avoidance trajectory planning method, characterized in that: The following steps are involved: S1, collects environmental data through solid-state laser radar and binocular camera, and builds a 3D point cloud map based on 3D mapping and positioning algorithms; S2, using the robot’s front binocular camera based on the SGBM algorithm, determines the distance from the obstacle to the robot’s current center of mass; S3, taking the center of the rear axle of the robot as the origin, establish the robot's own Cartesian coordinate system with the robot's motion direction as the longitudinal direction and the x-axis as the y-axis. Based on the robot's own Cartesian coordinate system, calculate the coordinates of the four vertices of the robot in the clockwise direction, and convert them to the global Cartesian coordinate system. Extract the two-dimensional plane coordinates of all point clouds, and use the two linear equations as boundaries to filter out the obstacle point clouds within the boundaries. S4, calculating the longitudinal distances from all obstacle point clouds to the robot, retaining the obstacle point clouds with the preset number of samplings of the closest longitudinal distances as the obstacle effective boundary point cloud set at this time according to the preset number of obstacle point cloud samplings; then fitting the effective boundary point clouds based on the cubic spline curve to generate the obstacle effective boundary curve at the current moment; S5, based on the Lattice Planner trajectory planning algorithm, generates the robot's local candidate trajectory through scattered point sampling and quintic polynomial trajectory fitting; S6, by judging the relationship between each candidate trajectory and the effective boundary curve of the current obstacle, if there is an intersection, it is deleted, otherwise, it is retained; S7, based on the principle of minimum cost, compare all candidate trajectories that do not intersect with the effective boundary curve of the current obstacle to determine the local optimal motion trajectory; S8, publishing the path point position information of the motion trajectory to the control layer, transmitting the control signal to the servo motor driver, controlling the hydraulic oil pump to work, and then making the hydraulic motor work; S9, at any time 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 method for obstacle avoidance trajectory planning of a nuclear facility decommissioning robot according to claim 1, characterized in that: In step S2, the distance from the obstacle to the current center of mass of the robot is calculated 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) Where 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 the local Cartesian coordinate system to the global Cartesian coordinate system is: In the formula, represents the position of the robot in global Cartesian coordinates, represents the vertex coordinates of the robot in its own Cartesian coordinate system, where i = 1, 2, 3, 4; θ represents the robot's heading angle, represents the vertex coordinates of the robot in the global Cartesian coordinate system; in the global Cartesian coordinate system, the vertices and vertices Perform linear fitting and obtain two straight line equations: R represents an arbitrary 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 obstacle effective boundary curve at the current moment is generated as: 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 represents the spline coefficient of the i-th segment curve, S τi (t) represents the i-th curve corresponding to time t in the τ direction, and τ represents the x direction or the y direction.
5. The method for obstacle avoidance trajectory planning of a nuclear facility decommissioning robot according to claim 4, characterized in that: In step S5, the generated local candidate trajectory of the robot is: In the formula: a0, a1, a2, a3, a4, and a5 represent the coefficients of the quintic polynomial, and t is the time parameter.
Citation Information
Patent Citations
Track planning method based on time optimal motion primitive, terminal and storage medium
CN116301024A
Obstacle avoidance re-planning method and system considering dynamic constraints and operation limiting conditions
CN116540722A
Intelligent vehicle multi-constraint trajectory planning method based on multi-dimensional laser radar point cloud information
CN116991159A
Robot real-time obstacle avoidance and dynamic path planning method and system
CN117970925A
Obstacle recognition method for autonomous robots
US20220066456A1
Cited By
Narrow space trajectory generation method and device without prior SLAM (Simultaneous Localization and Mapping) map
CN119555053A
Obstacle avoidance method and system for unmanned aerial vehicle
CN120872001A
