A laser break-in robot motion path planning and obstacle avoidance system
By integrating a modular path planning and obstacle avoidance system, the problems of perception error, laser risk and task planning separation of laser demolition robots in high-risk environments have been solved, achieving autonomous, safe and high-quality task execution.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- SHENYANG FIRE RES INST OF MEM
- Filing Date
- 2026-04-29
- Publication Date
- 2026-06-02
AI Technical Summary
Existing laser demolition robot path planning methods suffer from problems such as idealized perception and control, simplistic laser safety models, separation of task and planning, and static and fragmented planning methods, making it impossible to safely execute high-quality tasks in high-risk and complex environments.
By comprehensively applying the calibration and uncertainty module, environment representation module, risk constraint module, task modeling module, global coarse planning module, trajectory optimization module, and dynamic obstacle avoidance module, a complete technical closed loop from sensor noise modeling to operation quality prediction is achieved, unifying the constraints on laser risks and enabling autonomous planning and safe execution of tasks.
It provides precise spatial reference, multi-level environmental representation, secure beam path management, mathematical integration of mission and planning, and real-time response to dynamic obstacles, ensuring safe and high-quality mission execution in high-risk environments.
Smart Images
Figure CN122125722A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of robot motion path planning technology, and in particular to a laser demolition robot motion path planning and obstacle avoidance system. Background Technology
[0002] Laser demolition robots are robotic systems used for targeted demolition, cutting, drilling, and penetration of specific materials. They typically include a laser emission and optical path system, a robot body and actuators, a perception and positioning system, a control and safety system, and a task planning layer. The motion path of a laser demolition robot is the trajectory generated for the robot from its starting point to its endpoint. Existing path planning methods include sampling-based path planning, graph search-based planning, and task space planning, but these methods have the following drawbacks: 1) Idealization of perception and control: Most methods ignore calibration errors and positioning uncertainties, resulting in rigid and inaccurate perception maps that cannot be used for precise risk calculations. 2) Simplified laser safety models: These models treat the laser head only as a physical obstacle for geometric obstacle avoidance, completely failing to handle the risk of direct laser beam irradiation and complex specular reflection secondary damage. 3) Separation of task, model, and planning: Cutting process requirements are separated from robot motion planning, often executed according to fixed parameters, making it impossible to adjust the robot's position and speed online according to complex environments to simultaneously ensure safety and quality. 4) Static and fragmented planning methods: The chassis and robotic arm are planned separately or simply coupled, which makes it difficult to handle continuous movement operation tasks with strict attitude constraints, and is helpless against dynamic obstacles. Summary of the Invention
[0003] To overcome the shortcomings of existing technologies, the purpose of this invention is to provide a motion path planning and obstacle avoidance system for laser demolition robots. This system realizes a complete technical closed loop, from sensor noise modeling to job quality prediction and unified constraints on dynamic and static laser risks. This enables mobile laser demolition robots to autonomously plan and safely execute high-quality tasks in high-risk and complex environments.
[0004] To achieve the above objectives, the present invention provides the following solution: a laser demolition robot motion path planning and obstacle avoidance system, comprising: The calibration and uncertainty module is used to construct a comprehensive coordinate system based on the robot positioning system, and calculate the external parameters of the laser head and end effector, the external parameters of the radar and chassis, and the statistical error based on the comprehensive coordinate system to obtain multi-source calibration and uncertainty parameters. The environment representation module is used to perform point cloud unification, geometric layer determination, surface extraction, and surface reflection attribute priors using the multi-source calibration and uncertainty parameters to obtain an environment representation for geometric collision detection and laser risk calculation. The risk constraint module is used to define the candidate robot state and the risk events of laser beam emission during the path planning process, in order to calculate the direct irradiation risk and reflection risk of the main laser beam and obtain the risk constraints. The task modeling module is used to perform reference contour parameterization, attitude constraints, and velocity and dwell time constraints based on the cutting task, so as to obtain an optimized cutting task. The cutting model module is used to predict the total cutting depth based on the energy deposition model according to the risk constraints and the optimized cutting task, and to complete the construction of the quality predictor. The global coarse planning module constructs a task access set based on the environment representation, the risk constraints, and the quality predictor, and then performs a graph search based on the task access set to obtain the global candidate access sequence for the chassis. The trajectory optimization module is used to set the cost function and hard constraint set based on the global candidate access sequence of the chassis in order to perform segmented temporal trajectory optimization of the robotic arm and obtain an executable optimized temporal trajectory. The dynamic obstacle avoidance module constructs a local controller based on the executable optimized temporal trajectory, and uses the local controller to perform trajectory safety control to complete dynamic obstacle avoidance; The calibration and uncertainty module, the environment representation module, the risk constraint module, the task modeling module, the cutting model module, the global coarse planning module, the trajectory optimization module, and the dynamic obstacle avoidance module are interconnected.
[0005] Optionally, the calibration and uncertainty module includes: The coordinate and calibration unit is used to construct a comprehensive coordinate system based on the robot positioning system, fix a rigid body calibration plate based on the comprehensive coordinate system, and set a detectable checkerboard corner point on the rigid body calibration plate. The first offline calibration unit is used to establish the correspondence between laser point images and TCP poses through attitude sampling based on the integrated coordinate system and the rigid body calibration plate, perform least squares estimation on all sampling points according to the correspondence to obtain the laser head and end effector extrinsic parameters, and then calculate the residual covariance of the calibration points according to the laser head and end effector extrinsic parameters to obtain the laser point pose uncertainty. The second offline calibration unit is used to perform planar motions of different postures on the chassis of the laser demolition robot to synchronously record the original observations of the sensors and the estimates of the chassis odometer and IMU to obtain sampled data. Based on the sampled data, the radar and chassis are jointly optimized by using nonlinear optimization to minimize the reprojection error to obtain the external parameters of the radar and chassis. The joint error unit is used to perform repeated positioning tests on the robotic arm to obtain state estimation and joint coding error, and then integrate the pose uncertainty and the joint coding error into a statistical error. The parameter integration unit is used to integrate the laser head and end external parameters, the radar and chassis external parameters, and the statistical error to obtain multi-source calibration and uncertainty parameters.
[0006] Optionally, the integrated coordinates include a map coordinate system, a mobile chassis coordinate system, a laser head coordinate system, a robotic arm end-effector center point coordinate system, a camera coordinate system, and a lidar coordinate system.
[0007] Optionally, the environment representation module includes: A point cloud unification unit is used to perform homogeneous transformation on the observed point cloud using the multi-source calibration and uncertainty parameters, so as to align the observed point cloud in the map coordinate system. The geometric layer determination unit is used to establish a three-dimensional voxel grid in the map coordinate system, calculate the occupancy probability of any spatial location in each grid, and obtain a geometric map including three-dimensional occupancy. The surface extraction unit is used to extract the mesh in the TSDF based on the geometric map, perform clustering operations on the extracted mesh to obtain a set of patches, calculate the normal and boundary of each patch, and output the set of surfaces for subsequent laser ray intersection calculation. A reflection semantic unit is used to divide the reflection semantics into allowed areas, prohibited reflection areas and high reflectivity material areas based on the surface set, and to define material prior rules, sensor prior reflection indices and task area segmentation to obtain initialization rules. Based on the initialization rules, the material categories are updated online using vision and Bayesian methods to assign reflection attributes to each surface, thereby obtaining a reflection semantic map. The obstacle avoidance input unit is used to integrate the geometric map, the surface set, the reflection semantic map, and the predicted obstacle state trajectory and uncertainty to obtain an environmental representation for geometric collision detection and laser risk calculation.
[0008] Optionally, the risk constraint module includes: Candidate state units are used to define the chassis pose, robotic arm joint angles, and TCP pose of the laser demolition robot at each moment during the path planning process, so as to obtain the candidate robot state. The risk event unit is used to define the main beam irradiating a non-target area as a direct action event and the reflected beam generated after the main beam irradiates the surface and enters the prohibited area as a reflection action event, based on the candidate robot's state and time, to obtain the risk events of laser beam emission. The direct irradiation risk unit is used to calculate the ray corresponding to the main laser beam based on the direct action event and each candidate state in the candidate robot states, find the intersection of the ray corresponding to the main laser beam and the triangle of the TSDF extracted mesh to obtain the first hit patch, determine whether the first hit patch belongs to the prohibited direct irradiation area in the surface set, and obtain the direct irradiation risk value. The reflection ray tracing unit is used to determine whether the main laser beam hits the high reflectivity surface based on the reflection event. If so, the reflection path is calculated to obtain the reflection risk value. Planning constraint units are used to integrate direct illumination risk values and reflection paths into a computable risk constraint.
[0009] Optionally, the task modeling module includes: The cutting contour unit is used to obtain the cutting path based on the cutting task, smooth and resample the cutting path, parameterize it as an arc length parameter, obtain the cutting contour curve including the reference point and the cutting plane direction, and define the reference pose that TCP needs to track based on the cutting contour curve. The multi-constraint unit is used to calculate the normal angle deviation and focal distance based on the cutting contour curve to obtain the relative attitude constraint of TCP, and to obtain the speed and dwell time constraint along the parameter speed, minimum speed range, maximum speed range and acceleration limit of the cutting contour curve. The qualified constraint unit is used to describe the relative attitude constraint and the velocity and dwell time constraint as a qualified feasible region, and to define an efficiency-first objective function, a quality-first objective function and an energy-first objective function in the qualified feasible region, and output the optimized cutting task.
[0010] Optionally, the cutting model module includes: The cutting depth prediction unit is used to calculate the effective energy deposition intensity based on the laser power according to the energy deposition model, calculate the cutting depth increment based on the effective energy deposition intensity, and then calculate the predicted total cutting depth based on the cutting depth increment and the target cutting depth required by the process; wherein: The calculation expression for the cutting depth increment is: ; in, This represents the increment of cutting depth along the arc length. This is the proportionality coefficient. This is the incident angle correction factor. For laser power, The cutting speed along the contour. It is an exponential function; The lightweight calibration unit is used to measure the cutting depth using a vision camera and a laser rangefinder. Based on the measured cutting depth, it uses a regression algorithm to update the scaling factor, incident angle correction factor, and power function online to complete the construction of the quality predictor.
[0011] Optionally, the global coarse planning module includes: The task access set unit is used to filter the set of access chassis poses that can complete the cutting in the candidate robot state for each cutting contour point or cutting contour segment, using the environment representation, the risk constraint and the quality predictor, to obtain the task access set. The global search unit is used to set the candidate access points of each segment as graph nodes based on the task access set and combined with the multi-resolution grid algorithm, set the feasible chassis driving path from the previous access point to the next access point as the edge of the graph, and determine the feasibility of the edge to obtain the feasible route. The objective function unit is used to obtain a global objective function based on the feasible routes, combined with the weighted fusion edge geometric cost, static collision cost, and access point LRD risk cost. Based on the global objective function, the minimum cost path is selected from the feasible routes to obtain a global candidate access sequence.
[0012] Optionally, the trajectory optimization module includes: An optimization variable definition unit is used to divide the set total task duration into multiple segmented time domains based on the global candidate access sequence, and to define optimization variables for the segmented time domains; the optimization variables include chassis pose, robotic arm joint angle, end effector TCP pose, and contour movement speed; The cost and constraint unit is used to weightedly fuse the cutting contour tracking cost, smoothing cost, quality error cost and risk cost based on the segmented time domain to obtain a cost function, and to define collision constraints, joint constraints, TCP attitude constraints, laser risk constraints and cutting quality constraints to obtain a set of hard constraints. The solution unit is used to calculate the optimal trajectory in the piecewise time domain based on the cost function and the set of hard constraints using a constraint optimization solver, so as to obtain an executable optimized time-series trajectory.
[0013] Optionally, the dynamic obstacle avoidance module includes: The local execution hot start unit is used to predict the future trajectory of dynamic obstacles based on the executable optimized time trajectory, and define the feasible speed set of the chassis and the feasible joint speed set of the robotic arm to obtain the local controller; The online cyclic execution unit is used to obtain the robot's current state and attitude uncertainty based on the local controller, use the current state and attitude uncertainty to perform trajectory tracking and prediction, and use the laser risk barrier function to perform safety verification on the predicted trajectory in order to perform trajectory safety control and complete dynamic obstacle avoidance.
[0014] This invention discloses the following technical effects by providing a laser demolition robot motion path planning and obstacle avoidance system: 1. By integrating the extrinsic parameter calibration and pose uncertainty of the laser head and end effector, radar and chassis, and joint coding error into a unified multi-source calibration and uncertainty parameter; in particular, through attitude sampling, least squares estimation, and covariance calculation, the uncertainty of the laser point pose is quantified. This provides a precise spatial reference with confidence, enabling the system to be aware of its perception and positioning error range.
[0015] 2. By integrating 3D occupied geometry, extracted surface sets, and a reflection semantic map with prior material reflection properties and Bayesian online updates, a unified, multi-level environmental representation input is provided for geometric collision detection and laser risk calculation. In particular, it can distinguish highly reflective materials and prohibited reflection zones, enabling the system to predict reflection risks.
[0016] 3. Defines the risks of direct beam irradiation of non-target areas (direct action risk event) and reflection beam entering prohibited areas (reflection action risk event) in laser demolition; and quantifies these two types of risks into calculable constraints through ray intersection and reflection path tracing; can achieve safety protection for people and equipment, upgrading from physical separation to beam path management, and can actively avoid the dangers caused by direct irradiation and specular reflection.
[0017] 4. By parameterizing the cutting task into an arc length parameter model with multiple constraints, and defining a qualified feasible region and a multi-objective function that includes TCP relative attitude, velocity, and acceleration constraints, it is possible to transform abstract process requirements such as cutting quality, efficiency, and energy consumption into mathematically optimizable optimization problems with boundaries, thus realizing the mathematical connection between task and planning.
[0018] 5. By constructing an energy deposition model to predict the cutting depth, it is possible to predict whether the cutting effect will meet the standard based on speed and power during the planning stage, thereby making adjustments in advance during optimization and achieving quality-oriented planning.
[0019] 6. By constructing a task access set concept and establishing a graph search model based on it, the feasibility of the chassis's ability to access the cutting task in various poses and move between access points is transformed into edges of graph nodes. Geometric cost, collision cost, and laser risk cost are then integrated for global optimization. This efficiently solves the problem of finding the optimal chassis positioning sequence for continuous, posture-required cutting tasks in complex environments by a mobile robotic arm.
[0020] 7. Through trajectory optimization, smooth, executable optimal temporal trajectories that satisfy all safety, kinematic, and mass constraints can be generated, truly achieving optimal motion planning under multi-objective constraints. Based on the optimized trajectory, a warm start is performed, and a local controller incorporating a laser risk barrier function is constructed. While tracking the trajectory, the risk barrier function is used to verify and correct the tracking behavior in real time to cope with dynamic obstacles, ensuring millisecond-level dynamic laser safety response capability while executing high-value optimized trajectories, thus balancing optimal planning and real-time safety.
[0021] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. Attached Figure Description
[0022] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.
[0023] Figure 1 This is a schematic diagram of the system architecture provided in an embodiment of the present invention; Figure 2 This is a schematic diagram of the cutting depth prediction process provided in an embodiment of the present invention; Figure 3 This is a schematic diagram of the path planning and obstacle avoidance process provided in an embodiment of the present invention. Detailed Implementation
[0024] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.
[0025] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.
[0026] like Figure 1 As shown, this invention provides a laser demolition robot motion path planning and obstacle avoidance system, comprising: 1. For example Figure 2 As shown, the calibration and uncertainty module is used to construct a comprehensive coordinate system based on the robot positioning system. Based on the comprehensive coordinate system, it calculates the extrinsic parameters of the laser head and end effector, the extrinsic parameters of the radar and chassis, and statistical errors to obtain multi-source calibration and uncertainty parameters. The calibration and uncertainty module includes: 1.1 Coordinate and calibration unit, used to construct a comprehensive coordinate system based on the robot positioning system, fix a rigid body calibration plate based on the comprehensive coordinate system, and set a detectable checkerboard corner point on the rigid body calibration plate; the comprehensive coordinate system includes a map coordinate system, a mobile chassis coordinate system, a laser head coordinate system, a robotic arm end-effector center point coordinate system, a camera coordinate system, and a lidar coordinate system.
[0027] 1.2 The first offline calibration unit is used to establish the correspondence between laser point images and TCP poses through attitude sampling based on the integrated coordinate system and the rigid body calibration plate, perform least squares estimation on all sampling points according to the correspondence to obtain the laser head and end effector extrinsic parameters, and then calculate the residual covariance of the calibration points according to the laser head and end effector extrinsic parameters to obtain the laser point pose uncertainty.
[0028] 1.3 The second offline calibration unit is used for laser-based demolition robots to perform planar motions in different postures using the chassis. It synchronously records the original observations of the sensors as point cloud features and synchronously records the chassis odometer and IMU estimates as the initial pose to obtain sampled data. Based on the sampled data, it uses nonlinear optimization to minimize the reprojection error and jointly optimizes the radar and chassis. The goal is to make the observed features coincide in a unified map coordinate system and obtain the extrinsic parameters of the radar and chassis.
[0029] 1.4 Joint error unit, used to perform repeated positioning tests on the robotic arm to obtain state estimation and joint coding error, and then integrate the pose uncertainty and the joint coding error into statistical error.
[0030] Repeated positioning experiment: Select several typical joint configurations, repeatedly send the same joint target each time, record the actual joint angles, estimate the variance of each joint and form a covariance matrix. If a unified global form is required, the statistics of all posture samples can be combined.
[0031] 1.5 Parameter integration unit, used to integrate the laser head and end external parameters, the radar and chassis external parameters and the statistical error to obtain multi-source calibration and uncertainty parameters.
[0032] 2. For example Figure 2As shown, the environment representation module is used to perform point cloud unification, geometric layer determination, surface extraction, and surface reflection attribute priors using the multi-source calibration and uncertainty parameters to obtain an environment representation for geometric collision detection and laser risk calculation; the environment representation module includes: 2.1 Point cloud unification unit, used to perform homogeneous transformation on the observed point cloud using the multi-source calibration and uncertainty parameters, so as to align the observed point cloud in the map coordinate system.
[0033] 2.2 Geometric layer determination unit, used to establish a three-dimensional voxel grid in the map coordinate system, calculate the occupancy probability of any spatial location in each grid, and obtain a geometric map including three-dimensional occupancy.
[0034] 2.3 Surface extraction unit, used to extract meshes in TSDF based on the geometric map, perform clustering operations on the extracted meshes to obtain a set of facets, calculate the normal and boundary of each facet, and output the set of surfaces for subsequent laser ray intersection calculation.
[0035] TSDF: Calculate the distance to the nearest surface for each voxel, truncate the distance with a truncation threshold, fuse multiple frames to obtain a stable surface, and extract triangular meshes after outputting TSDF voxels.
[0036] 2.4 Reflection semantic unit, used to divide the reflection semantics into allowed areas, prohibited reflection areas and high reflectivity material areas based on the surface set, and define material prior rules, sensor prior reflection indices and task area segmentation to obtain initialization rules. According to the initialization rules, the material categories are updated online using vision and Bayesian methods to assign reflection attributes to each surface, thus obtaining a reflection semantic map.
[0037] Permissible zones: For example, lasers are permitted near the surface of the target material; No-reflection zones: For example, reflective surfaces behind areas where people might be present; Areas with highly reflective materials: such as metal frames, glass, painted walls, etc.
[0038] 2.5 Obstacle avoidance input unit, used to integrate the geometric map, the surface set, the reflection semantic map, and the predicted obstacle state trajectory and uncertainty to obtain an environmental representation for geometric collision detection and laser risk calculation.
[0039] 3. For example Figure 2 As shown, the risk constraint module is used to define the candidate robot states and risk events of laser beam emission during the path planning process, in order to calculate the direct irradiation risk and reflection risk of the main laser beam, and obtain risk constraints; the risk constraint module includes: 3.1 Candidate state unit, used to define the chassis pose, robotic arm joint angle and TCP pose of the laser demolition robot at each moment during the path planning process, to obtain the candidate robot state.
[0040] 3.2 Risk event unit, used to define the main beam irradiating a non-target area as a direct action event and the reflected beam generated after the main beam irradiates the surface entering the prohibited area as a reflection action event, based on the candidate robot state and time, to obtain the risk events of laser beam emission.
[0041] 3.3 Direct Irradiation Risk Unit, used to calculate the ray corresponding to the main laser beam based on the direct action event and each candidate state in the candidate robot states, find the intersection of the ray corresponding to the main laser beam and the triangle of the TSDF extracted mesh to obtain the first hit patch, determine whether the first hit patch belongs to the prohibited direct irradiation area in the surface set, and obtain the direct irradiation risk value.
[0042] 3.4 The reflection ray tracing unit is used to determine whether the main laser beam hits the high reflectivity surface based on the reflection event. If so, the reflection path is calculated to obtain the reflection risk value.
[0043] 3.5 Planning constraint element, used to integrate the direct illumination risk value and reflection path into a computable risk constraint.
[0044] 4. For example Figure 2 As shown, the task modeling module is used to perform reference contour parameterization, attitude constraints, and velocity and dwell time constraints based on the cutting task to obtain an optimized cutting task; the task modeling module includes: 4.1 The cutting contour unit is used to obtain the cutting path based on the cutting task, smooth and resample the cutting path, parameterize it as an arc length parameter, obtain a cutting contour curve including a reference point and the cutting plane direction, and define the reference pose that TCP needs to track based on the cutting contour curve. The cutting contour curve is the complete boundary line of laser cutting, and the definition of the cutting contour curve is to transform the closed boundary line into a spatial curve coordinate that the robot can recognize.
[0045] 4.2 A multi-constraint unit is used to calculate the normal angle deviation and focal distance based on the cutting contour curve to obtain the relative attitude constraints of the TCP, and to obtain the speed and dwell time constraints along the parameter speed, minimum speed range, maximum speed range, and acceleration limit of the cutting contour curve; wherein, the minimum speed range ensures energy deposition, the maximum speed range avoids overheating, and the acceleration limit avoids mechanical vibration. Normal angle deviation: the deviation angle of the laser nozzle perpendicular to the wall surface; Focal distance: the distance between the nozzle and the wall surface.
[0046] 4.3 Qualified constraint unit, used to describe the relative attitude constraint and the velocity and dwell time constraint as a qualified feasible region, and define an efficiency-first objective function, a quality-first objective function and an energy-first objective function in the qualified feasible region, and output the optimized cutting task.
[0047] 5. For example Figure 2 As shown, the cutting model module is used to predict the total cutting depth based on the energy deposition model according to the risk constraints and the optimized cutting task, thus completing the construction of the quality predictor; the cutting model module includes: 5.1 Cutting depth prediction unit, used to calculate the effective energy deposition intensity based on the laser power according to the energy deposition model, calculate the cutting depth increment based on the effective energy deposition intensity, and then calculate the predicted total cutting depth based on the cutting depth increment and the target cutting depth required by the process; wherein: The calculation expression for the cutting depth increment is: ; in, This represents the increment of cutting depth along the arc length. This is the proportionality coefficient. This is the incident angle correction factor. For laser power, The cutting speed along the contour is the maximum speed; the lower the speed, the longer the unit arc time, and the stronger the deposition. It is an exponential function; Typically, the angle deviation between the TCP normal and the workpiece normal, or the angle of the laser incident relative to the ideal direction, is taken.
[0048] 5.2 Lightweight calibration unit is used to measure the cutting depth using a vision camera and a laser rangefinder, and then, based on the measured cutting depth, uses a regression algorithm to update the scaling factor, incident angle correction factor, and power function online to complete the construction of the quality predictor.
[0049] 6. For example Figure 3 As shown, the global coarse planning module constructs a task access set based on the environment representation, the risk constraints, and the quality predictor, and then performs a graph search based on the task access set to obtain a global candidate access sequence for the chassis; the global coarse planning module includes: 6.1 The task access set unit is used to filter the set of access chassis poses that can complete the cutting in the candidate robot state for each cutting contour point or cutting contour segment using the environment representation, the risk constraint and the quality predictor, so as to obtain the task access set.
[0050] 6.2 Global search unit, used to set the candidate access points of each segment as graph nodes based on the task access set and combined with the multi-resolution grid algorithm, set the feasible chassis driving path from the previous access point to the next access point as the edge of the graph, and determine the feasibility of the edge to obtain the feasible route.
[0051] 6.3 Objective function unit, used to obtain a global objective function based on the feasible routes, combined with weighted fusion edge geometric cost, static collision cost and access point LRD risk cost, and to select the minimum cost path among the feasible routes according to the global objective function to obtain a global candidate access sequence.
[0052] 7. For example Figure 3 As shown, the trajectory optimization module is used to set a cost function and a set of hard constraints based on the global candidate access sequence of the chassis, so as to perform segmented temporal trajectory optimization of the robotic arm and obtain an executable optimized temporal trajectory; the trajectory optimization module includes: 7.1 An optimization variable definition unit, used to divide the set total task duration into multiple segmented time domains based on the global candidate access sequence, and to define optimization variables for the segmented time domains; the optimization variables include: Chassis position: at each point in time, the chassis's XY coordinates and the direction the vehicle is facing in the field; Robotic arm joint angles: The rotation angles of the six joints of the robotic arm at each point in time; End TCP pose: The position and orientation of the laser gun nozzle TCP at each time point; Movement speed along the contour: The movement speed at each position along the cutting contour, and the total duration of the entire process.
[0053] 7.2 Cost and Constraint Unit, used to weightedly fuse cutting contour tracking cost, smoothing cost, quality error cost and risk cost based on the segmented time domain to obtain cost function, and define collision constraints, joint constraints, TCP attitude constraints, laser risk constraints and cutting quality constraints to obtain a set of hard constraints.
[0054] Collision constraints: The robot's chassis and robotic arm must maintain a minimum safe distance from obstacles such as walls, pillars, and equipment at every moment of movement; Joint constraints: Each joint of the robotic arm cannot rotate beyond its physical limit angle; the rotation speed of the joints cannot exceed the maximum value, otherwise the motor will directly trigger an overload alarm and stop the machine. TCP attitude constraint: The angle between the laser gun nozzle and the perpendicular direction of the cutting surface at every instant of motion cannot exceed a predetermined maximum value; Laser risk constraint: At every moment of robot movement, the probability of direct laser beam or laser bounce must not exceed the predetermined safety limit. Cutting quality constraint: For each point on the cutting profile, the predicted cumulative cutting depth must at least reach the required depth.
[0055] 7.3 Solver unit, used to calculate the optimal trajectory in the piecewise time domain based on the cost function and the set of hard constraints using a constraint optimization solver, to obtain an executable optimized time-series trajectory.
[0056] 8. For example Figure 3 As shown, the dynamic obstacle avoidance module constructs a local controller based on the executable optimized temporal trajectory, and uses the local controller to perform trajectory safety control to complete dynamic obstacle avoidance; the dynamic obstacle avoidance module includes: 8.1 Local execution hot start unit, used to predict the future trajectory of dynamic obstacles based on the executable optimized time trajectory, and define the chassis feasible speed set and the robotic arm feasible joint speed set to obtain a local controller; 8.2 Online cyclic execution unit, used to obtain the robot's current state and attitude uncertainty based on the local controller, use the current state and attitude uncertainty to perform trajectory tracking and prediction, and use the laser risk barrier function to perform safety verification on the predicted trajectory in order to perform trajectory safety control and complete dynamic obstacle avoidance.
[0057] Therefore, this invention provides a laser demolition robot motion path planning and obstacle avoidance system, realizing a complete technical closed loop from sensor noise modeling to operation quality prediction and unified constraints on dynamic and static laser risks, enabling the mobile laser demolition robot to autonomously plan and safely execute high-quality tasks in high-risk and complex environments.
[0058] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on the differences from other embodiments. The same or similar parts between the various embodiments can be referred to each other.
[0059] This document uses specific examples to illustrate the principles and implementation methods of the present invention. The descriptions of the above embodiments are only for the purpose of helping to understand the method and core ideas of the present invention. Furthermore, those skilled in the art will recognize that, based on the ideas of the present invention, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of the present invention.
Claims
1. A laser demolition robot motion path planning and obstacle avoidance system, characterized in that, include: The calibration and uncertainty module is used to construct a comprehensive coordinate system based on the robot positioning system, and calculate the external parameters of the laser head and end effector, the external parameters of the radar and chassis, and the statistical error based on the comprehensive coordinate system to obtain multi-source calibration and uncertainty parameters. The environment representation module is used to perform point cloud unification, geometric layer determination, surface extraction, and surface reflection attribute priors using the multi-source calibration and uncertainty parameters to obtain an environment representation for geometric collision detection and laser risk calculation. The risk constraint module is used to define the candidate robot state and the risk events of laser beam emission during the path planning process, in order to calculate the direct irradiation risk and reflection risk of the main laser beam and obtain the risk constraints. The task modeling module is used to perform reference contour parameterization, attitude constraints, and velocity and dwell time constraints based on the cutting task, so as to obtain an optimized cutting task. The cutting model module is used to predict the total cutting depth based on the energy deposition model according to the risk constraints and the optimized cutting task, and to complete the construction of the quality predictor. The global coarse planning module constructs a task access set based on the environment representation, the risk constraints, and the quality predictor, and then performs a graph search based on the task access set to obtain the global candidate access sequence for the chassis. The trajectory optimization module is used to set the cost function and hard constraint set based on the global candidate access sequence of the chassis in order to perform segmented temporal trajectory optimization of the robotic arm and obtain an executable optimized temporal trajectory. The dynamic obstacle avoidance module constructs a local controller based on the executable optimized temporal trajectory, and uses the local controller to perform trajectory safety control to complete dynamic obstacle avoidance; The calibration and uncertainty module, the environment representation module, the risk constraint module, the task modeling module, the cutting model module, the global coarse planning module, the trajectory optimization module, and the dynamic obstacle avoidance module are interconnected.
2. The laser demolition robot motion path planning and obstacle avoidance system according to claim 1, characterized in that, The calibration and uncertainty module includes: The coordinate and calibration unit is used to construct a comprehensive coordinate system based on the robot positioning system, fix a rigid body calibration plate based on the comprehensive coordinate system, and set a detectable checkerboard corner point on the rigid body calibration plate. The first offline calibration unit is used to establish the correspondence between laser point images and TCP poses through attitude sampling based on the integrated coordinate system and the rigid body calibration plate, perform least squares estimation on all sampling points according to the correspondence to obtain the laser head and end effector extrinsic parameters, and then calculate the residual covariance of the calibration points according to the laser head and end effector extrinsic parameters to obtain the laser point pose uncertainty. The second offline calibration unit is used to perform planar motions of different postures on the chassis of the laser demolition robot to synchronously record the original observations of the sensors and the estimates of the chassis odometer and IMU to obtain sampled data. Based on the sampled data, the radar and chassis are jointly optimized by using nonlinear optimization to minimize the reprojection error to obtain the external parameters of the radar and chassis. The joint error unit is used to perform repeated positioning tests on the robotic arm to obtain state estimation and joint coding error, and then integrate the pose uncertainty and the joint coding error into a statistical error. The parameter integration unit is used to integrate the laser head and end external parameters, the radar and chassis external parameters, and the statistical error to obtain multi-source calibration and uncertainty parameters.
3. The laser demolition robot motion path planning and obstacle avoidance system according to claim 2, characterized in that, The integrated coordinate system includes the map coordinate system, the mobile chassis coordinate system, the laser head coordinate system, the coordinate system of the center point of the robotic arm end effector, the camera coordinate system, and the lidar coordinate system.
4. The laser demolition robot motion path planning and obstacle avoidance system according to claim 3, characterized in that, The environment representation module includes: A point cloud unification unit is used to perform homogeneous transformation on the observed point cloud using the multi-source calibration and uncertainty parameters, so as to align the observed point cloud in the map coordinate system. The geometric layer determination unit is used to establish a three-dimensional voxel grid in the map coordinate system, calculate the occupancy probability of any spatial location in each grid, and obtain a geometric map including three-dimensional occupancy. The surface extraction unit is used to extract the mesh in the TSDF based on the geometric map, perform clustering operations on the extracted mesh to obtain a set of patches, calculate the normal and boundary of each patch, and output the set of surfaces for subsequent laser ray intersection calculation. A reflection semantic unit is used to divide the reflection semantics into allowed areas, prohibited reflection areas and high reflectivity material areas based on the surface set, and to define material prior rules, sensor prior reflection indices and task area segmentation to obtain initialization rules. Based on the initialization rules, the material categories are updated online using vision and Bayesian methods to assign reflection attributes to each surface, thereby obtaining a reflection semantic map. The obstacle avoidance input unit is used to integrate the geometric map, the surface set, the reflection semantic map, and the predicted obstacle state trajectory and uncertainty to obtain an environmental representation for geometric collision detection and laser risk calculation.
5. The laser demolition robot motion path planning and obstacle avoidance system according to claim 4, characterized in that, The risk constraint module includes: Candidate state units are used to define the chassis pose, robotic arm joint angles, and TCP pose of the laser demolition robot at each moment during the path planning process, so as to obtain the candidate robot state. The risk event unit is used to define the main beam irradiating a non-target area as a direct action event and the reflected beam generated after the main beam irradiates the surface and enters the prohibited area as a reflection action event, based on the candidate robot's state and time, to obtain the risk events of laser beam emission. The direct irradiation risk unit is used to calculate the ray corresponding to the main laser beam based on the direct action event and each candidate state in the candidate robot states, find the intersection of the ray corresponding to the main laser beam and the triangle of the TSDF extracted mesh to obtain the first hit patch, determine whether the first hit patch belongs to the prohibited direct irradiation area in the surface set, and obtain the direct irradiation risk value. The reflection ray tracing unit is used to determine whether the main laser beam hits the high reflectivity surface based on the reflection event. If so, the reflection path is calculated to obtain the reflection risk value. Planning constraint units are used to integrate direct illumination risk values and reflection paths into a computable risk constraint.
6. The laser demolition robot motion path planning and obstacle avoidance system according to claim 5, characterized in that, The task modeling module includes: The cutting contour unit is used to obtain the cutting path based on the cutting task, smooth and resample the cutting path, parameterize it as an arc length parameter, obtain the cutting contour curve including the reference point and the cutting plane direction, and define the reference pose that TCP needs to track based on the cutting contour curve. The multi-constraint unit is used to calculate the normal angle deviation and focal distance based on the cutting contour curve to obtain the relative attitude constraint of TCP, and to obtain the speed and dwell time constraint along the parameter speed, minimum speed range, maximum speed range and acceleration limit of the cutting contour curve. The qualified constraint unit is used to describe the relative attitude constraint and the velocity and dwell time constraint as a qualified feasible region, and to define an efficiency-first objective function, a quality-first objective function and an energy-first objective function in the qualified feasible region, and output the optimized cutting task.
7. The laser demolition robot motion path planning and obstacle avoidance system according to claim 6, characterized in that, The cutting model module includes: The cutting depth prediction unit is used to calculate the effective energy deposition intensity based on the laser power according to the energy deposition model, calculate the cutting depth increment based on the effective energy deposition intensity, and then calculate the predicted total cutting depth based on the cutting depth increment and the target cutting depth required by the process; wherein: The calculation expression for the cutting depth increment is: ； in, This represents the increment of the cutting depth along the arc length. This is the proportionality coefficient. This is the incident angle correction factor. For laser power, The cutting speed along the contour. It is an exponential function; The lightweight calibration unit is used to measure the cutting depth using a vision camera and a laser rangefinder. Based on the measured cutting depth, it uses a regression algorithm to update the scaling factor, incident angle correction factor, and power function online to complete the construction of the quality predictor.
8. The laser demolition robot motion path planning and obstacle avoidance system according to claim 7, characterized in that, The global coarse planning module includes: The task access set unit is used to filter the set of access chassis poses that can complete the cutting in the candidate robot state for each cutting contour point or cutting contour segment, using the environment representation, the risk constraint and the quality predictor, to obtain the task access set. The global search unit is used to set the candidate access points of each segment as graph nodes based on the task access set and combined with the multi-resolution grid algorithm, set the feasible chassis driving path from the previous access point to the next access point as the edge of the graph, and determine the feasibility of the edge to obtain the feasible route. The objective function unit is used to obtain a global objective function based on the feasible routes, combined with the weighted fusion edge geometric cost, static collision cost, and access point LRD risk cost. Based on the global objective function, the minimum cost path is selected from the feasible routes to obtain a global candidate access sequence.
9. The laser demolition robot motion path planning and obstacle avoidance system according to claim 8, characterized in that, The trajectory optimization module includes: An optimization variable definition unit is used to divide the set total task duration into multiple segmented time domains based on the global candidate access sequence, and to define optimization variables for the segmented time domains; the optimization variables include chassis pose, robotic arm joint angle, end effector TCP pose, and contour movement speed; The cost and constraint unit is used to weightedly fuse the cutting contour tracking cost, smoothing cost, quality error cost and risk cost based on the segmented time domain to obtain a cost function, and to define collision constraints, joint constraints, TCP attitude constraints, laser risk constraints and cutting quality constraints to obtain a set of hard constraints. The solution unit is used to calculate the optimal trajectory in the segmented time domain based on the cost function and the set of hard constraints using a constraint optimization solver, so as to obtain an executable optimized time-series trajectory.
10. The laser demolition robot motion path planning and obstacle avoidance system according to claim 9, characterized in that, The dynamic obstacle avoidance module includes: The local execution hot start unit is used to predict the future trajectory of dynamic obstacles based on the executable optimized time trajectory, and define the feasible speed set of the chassis and the feasible joint speed set of the robotic arm to obtain the local controller; The online cyclic execution unit is used to obtain the robot's current state and attitude uncertainty based on the local controller, use the current state and attitude uncertainty to perform trajectory tracking and prediction, and use the laser risk barrier function to perform safety verification on the predicted trajectory in order to perform trajectory safety control and complete dynamic obstacle avoidance.