Track generation method and device based on focal depth and width constraint, equipment and medium

Abstract

The invention relates to the technical field of laser application, and discloses a focal depth and width constraint-based trajectory generation method, device and equipment and a medium, and the method comprises the steps: obtaining model point clouds of a to-be-processed workpiece, sorting the model point clouds, and obtaining ordered point clouds; setting an initial iteration point cloud index according to the laser line width based on the ordered point cloud; calculating the length of the AABB bounding box from the current point to the starting point to obtain a first length; comparing the first length with the laser line width, and determining an end point index; performing iterative calculation according to the initial iterative point cloud index and the end point index to obtain an initial iterative point cloud; correcting the initial iteration point cloud by using an OBB bounding box according to the focal depth of the laser and the upper limit of the processing angle to obtain an initial track point and an initial track angle; updating the iteration condition, and judging whether the updated iteration condition is smaller than the point number of the model point cloud or not; and if not, ending iterative calculation, and obtaining a target trajectory point and a target trajectory angle. Reasonable trajectory planning is realized, and the operation precision and efficiency are effectively improved.

Description

Technical Field

[0001] The present invention relates to the technical field of laser applications, and in particular to a method, device, equipment and medium for generating a trajectory based on focal depth and width constraints. Background Art

[0002] Accurate trajectory planning is crucial in numerous laser-based applications, including 3D vision measurement, laser processing, and laser inspection. Traditional trajectory planning methods often fail to fully consider the physical characteristics of lasers, such as line width, focal depth, inclination angle, poor processing quality, and missed inspections. For example, in laser processing, improper trajectory planning can lead to uneven energy distribution within the laser element, compromising processing results. In 3D vision measurement, inappropriate trajectory planning can result in incomplete point cloud data acquisition and reduce the accuracy of reconstructed models. Therefore, an efficient trajectory planning algorithm that comprehensively considers laser characteristics is needed.

[0003] Due to its inherent physical properties, lasers have certain limitations, such as laser line width and focal depth. In addition, during the trajectory planning process, the robot's arm span, interference and other issues limit the angle of the posture. Therefore, based on the limited laser focal depth and width, as well as the robot's angle limitations during actual laser applications such as cleaning, inspection or processing, how to reasonably plan the trajectory to effectively improve operation accuracy and efficiency is a technical problem that needs to be solved urgently.

[0004] The above content is only used to assist in understanding the technical solution of the present invention and does not constitute an admission that the above content is prior art. Summary of the Invention

[0005] The main purpose of the present invention is to provide a method, device, equipment and medium for generating trajectories based on focal depth and width constraints, aiming to solve the technical problems in the prior art that the planning trajectory cannot be taken into account due to the limited focal depth and width of the laser, as well as the angle limitations of the robot during actual laser processing, resulting in low operation accuracy and efficiency.

[0006] To achieve the above object, the present invention provides a method for generating a trajectory based on focal depth and width constraints, the method comprising the following steps:

[0007] Acquire a model point cloud of a workpiece to be processed, and sort the model point cloud to obtain an ordered point cloud;

[0008] Based on the ordered point cloud, an initial iterative point cloud index is set according to the laser line width;

[0009] Calculate the length of the AABB bounding box from the current point to the starting point to obtain the first length;

[0010] comparing the first length with the laser line width to determine an end point index;

[0011] Performing iterative calculation according to the initial iterative point cloud index and the end point index to obtain an initial iterative point cloud;

[0012] According to the laser focal depth and the upper limit of the processing angle, the initial iterative point cloud is corrected using the OBB bounding box to obtain the initial trajectory point and the initial trajectory angle;

[0013] Updating the iteration condition, and determining whether the updated iteration condition is less than the number of points in the model point cloud;

[0014] If it is not less than, the iterative calculation ends and the target trajectory point and target trajectory angle are obtained.

[0015] Preferably, comparing the first length with the laser line width to determine the end point index comprises:

[0016] When the first length is less than the laser line width, determining whether the current point index is equal to the number of points in the ordered point cloud, and determining the end point index according to the determination result;

[0017] When the first length is greater than the laser line width, the recording end point index is the current index minus a first preset value.

[0018] Preferably, determining whether the current point index is equal to the number of points in the ordered point cloud and confirming the end point index according to the determination result includes:

[0019] If the current point index is equal to the number of points in the ordered point cloud, the current index is recorded as the end point index;

[0020] If the current point index is not equal to the number of points in the ordered point cloud, the current point index is updated, and the length of the AABB bounding box of the starting point of the current point is returned, and the step of obtaining the first length is continued to iterate.

[0021] Preferably, the initial iterative point cloud is corrected using an OBB bounding box according to the laser focal depth and the upper limit of the processing angle to obtain the initial trajectory points and the initial trajectory angle, including:

[0022] Calculating the OBB bounding box of the initial iterative point cloud;

[0023] Calculate the laser processing angle based on the OBB bounding box;

[0024] Calculate the length and width of the bounding box under the current pose;

[0025] Determining whether the laser processing angle is greater than the processing angle upper limit;

[0026] If the laser processing angle is less than the processing angle upper limit, recalibrate the center of the bounding box;

[0027] Determine whether the length of the bounding box in the current posture is less than the laser line width, and whether the width of the bounding box in the current posture is less than the laser focal depth;

[0028] If not, the initial iterative point cloud is updated according to the iteration step to obtain a new point cloud, the iteration condition is updated, and the length of the AABB bounding box of the starting point of the current point is returned to continue the iteration by obtaining the first length.

[0029] If satisfied, the iteration ends and the initial trajectory point and initial trajectory angle of the workpiece to be processed are output.

[0030] Preferably, after the step of determining whether the laser processing angle is greater than the upper limit of the processing angle, the method further comprises:

[0031] If the laser processing angle is greater than the processing angle upper limit, the processing angle upper limit is used as the current processing angle, a new bounding box pose is obtained, and the process returns to the step of calculating the length and width of the bounding box at the current pose.

[0032] Preferably, calculating the length and width of the bounding box at the current pose includes:

[0033] Convert the ordered point cloud to the current pose, and calculate the minimum and maximum points of the point cloud in the current pose;

[0034] The length and width of the bounding box at the current position are calculated based on the minimum and maximum points of the point cloud at the current position. Preferably, the angle of the laser processing is the angle between the y direction of the bounding box and the z axis of the coordinate system.

[0035] In addition, to achieve the above-mentioned purpose, the present invention further proposes a trajectory generation device based on focal depth and width constraints, the trajectory generation device based on focal depth and width constraints comprising:

[0036] A sorting module is used to obtain a model point cloud of a workpiece to be processed, sort the model point cloud, and obtain an ordered point cloud;

[0037] A setting module, configured to set an initial iterative point cloud index based on the ordered point cloud and according to the laser line width;

[0038] A calculation module, used to calculate the length of the AABB bounding box from the current point to the starting point to obtain a first length;

[0039] a comparison module, configured to compare the first length with the laser line width to determine an end point index;

[0040] The calculation module is further configured to perform iterative calculation based on the initial iterative point cloud index and the end point index to obtain an initial iterative point cloud;

[0041] A correction module is used to correct the initial iterative point cloud using an OBB bounding box according to the laser focal depth and the upper limit of the processing angle to obtain an initial trajectory point and an initial trajectory angle;

[0042] A judgment module, used for updating the iteration condition and judging whether the updated iteration condition is less than the number of points in the model point cloud;

[0043] The calculation module is further configured to terminate the iterative calculation if is not less than and obtain the target trajectory point and the target trajectory angle.

[0044] In addition, to achieve the above-mentioned purpose, the present invention also proposes a trajectory generation device based on focal depth and width constraints, wherein the trajectory generation device based on focal depth and width constraints includes a memory, a processor, and a trajectory generation program based on focal depth and width constraints stored in the memory and executable on the processor, wherein the trajectory generation program based on focal depth and width constraints is configured to implement the steps of the trajectory generation method based on focal depth and width constraints as described above.

[0045] In addition, to achieve the above-mentioned purpose, the present invention also proposes a storage medium, on which a program for generating a trajectory based on focal depth and width constraints is stored. When the program for generating a trajectory based on focal depth and width constraints is executed by a processor, the steps of the method for generating a trajectory based on focal depth and width constraints as described above are implemented.

[0046] In the present invention, a model point cloud of the workpiece to be processed is obtained, and the model point cloud is sorted to obtain an ordered point cloud. Based on the ordered point cloud, an initial iterative point cloud index is set according to the laser line width. The length of the AABB bounding box from the current point to the starting point is calculated to obtain a first length. The first length is compared with the laser line width to determine the end point index. An iterative calculation is performed based on the initial iterative point cloud index and the end point index to obtain an initial iterative point cloud. Based on the laser focal depth and the upper limit of the processing angle, the initial iterative point cloud is corrected using the OBB bounding box to obtain the initial trajectory point and initial trajectory angle. The iteration condition is updated to determine whether the updated iteration condition is less than the number of points in the model point cloud. If not, the iterative calculation is terminated to obtain the target trajectory point and target trajectory angle. Based on the limited laser focal depth and width, as well as the robot angle limit during processing, an effective envelope is generated. Based on the envelope, the target trajectory can be generated, which can effectively improve the operation accuracy and efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 Schematic diagram of the structure of a device for generating a trajectory based on focal depth and width constraints in a hardware operating environment according to an embodiment of the present invention;

[0048] Figure 21. A schematic flow chart of a first embodiment of a method for generating a trajectory based on depth of focus and width constraints according to the present invention;

[0049] Figure 3 This is an example diagram of a cross-sectional model point cloud in an embodiment of a method for generating a trajectory based on focal depth and width constraints of the present invention;

[0050] Figure 4 This is a flow chart of obtaining a model iterative point cloud in an embodiment of a trajectory generation method based on focal depth and width constraints of the present invention;

[0051] Figure 5 This is a flow chart of obtaining the fragmentation result in an embodiment of the method for generating a trajectory based on focal depth and width constraints of the present invention;

[0052] Figure 6 Schematic diagram of target trajectory points in an embodiment of the trajectory generation method based on focal depth and width constraints of the present invention;

[0053] Figure 7 This is a structural block diagram of a first embodiment of a trajectory generation device based on focal depth and width constraints according to the present invention;

[0054] The purpose, features and advantages of the present invention will be further described with reference to the accompanying drawings and in conjunction with the embodiments. DETAILED DESCRIPTION

[0055] It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0056] Reference Figure 1 , Figure 1 This is a schematic diagram of the structure of a trajectory generation device based on focal depth and width constraints in the hardware operating environment involved in an embodiment of the present invention.

[0057] like Figure 1As shown, the device for generating a trajectory based on focal depth and width constraints may include: a processor 1001, such as a central processing unit (CPU), a communication bus 1002, a user interface 1003, a network interface 1004, and a memory 1005. The communication bus 1002 is used to implement communication between these components. The user interface 1003 may include a display screen. Optionally, the user interface 1003 may also include a standard wired interface or a wireless interface. In the present invention, the wired interface of the user interface 1003 may be a USB interface. The network interface 1004 may optionally include a standard wired interface or a wireless interface (such as a wireless fidelity (WI-FI) interface). The memory 1005 may be a high-speed random access memory (RAM) or a non-volatile memory (NVM), such as a disk storage device. The memory 1005 may also be a storage device independent of the processor 1001.

[0058] Those skilled in the art will understand that Figure 1 The structure shown in the figure does not constitute a limitation to the apparatus for generating a trajectory based on focal depth and width constraints, and may include more or less components than shown in the figure, or combine certain components, or arrange the components differently.

[0059] like Figure 1 As shown, the memory 1005 as a computer storage medium may include an operating system, a network communication module, a user interface module, and a trajectory generation program based on focal depth and width constraints.

[0060] exist Figure 1 In the device for generating a trajectory based on focal depth and width constraints shown, the network interface 1004 is mainly used to connect to a backend server and communicate data with the backend server; the user interface 1003 is mainly used to connect to a user device; the device for generating a trajectory based on focal depth and width constraints calls a program for generating a trajectory based on focal depth and width constraints stored in the memory 1005 through the processor 1001, and executes the method for generating a trajectory based on focal depth and width constraints provided in an embodiment of the present invention.

[0061] Based on the above hardware structure, an embodiment of the present invention is proposed based on a method for generating a trajectory with focal depth and width constraints.

[0062] Reference Figure 2 , a first embodiment of the trajectory generation method based on depth of focus and width constraints of the present invention is proposed.

[0063] In a first embodiment, the method for generating a trajectory based on focal depth and width constraints includes the following steps:

[0064] Step S10: obtaining a model point cloud of the workpiece to be processed, and sorting the model point cloud to obtain an ordered point cloud.

[0065] It should be noted that the execution subject of this embodiment is the trajectory generation device based on the depth of focus and width constraints, wherein the trajectory generation device based on the depth of focus and width constraints can be an electronic device such as a personal computer or a server, and this embodiment does not limit this. The workpiece to be processed can be a workpiece to be cleaned by a laser, or a workpiece to be processed by a laser, laser detection, etc. For the sake of convenience, this embodiment takes the workpiece to be cleaned by a laser as an example for detailed description. Input the cross-sectional point cloud cloud_yoz_ptr as follows Figure 3 shown.

[0066] In the specific implementation, the input is not necessarily a point cloud, it can be any two-dimensional data.

[0067] Transfer the cross-section point cloud cloud_yoz_ptr to the xoz plane to obtain the converted model point cloud cloud_xoz_ptr.

[0068] Since the cross-section point cloud cloud_yoz_ptr is a point cloud on the yoz plane, its point is denoted as P1, with coordinates (0, y, z). Transform the cross-section point cloud cloud_yoz_ptr to the xoz plane to obtain the transformed model point cloud cloud_xoz_ptr, and denote its point as P2. The coordinates of P2 after transformation are (y, 0, z).

[0069] Sort the point cloud cloud_xoz_ptr in descending order of x to obtain the ordered point cloud cloud_xoz_sort_ptr.

[0070] Since the points in the point cloud cloud_xoz_ptr are unordered, the points need to be sorted. The sorting rules are as follows:

[0071] For the points in points, assume that there are points [Pt1, Pt2, Pt3, Pt4] in points, and their coordinates are Pt1(0.5, 0, 1), Pt2(0.6, 0, 0.8), Pt3(0.4, 0, 0.7), Pt4(0.55, 0, 1.1), and arrange the points Pt1, Pt2, Pt3, and Pt4 in descending order from large to small according to the value of x in the point coordinates, and finally output the result [Pt2, Pt4, Pt1, Pt3].

[0072] According to the above rules, the points in the point cloud cloud_xoz_ptr are sorted to obtain the ordered point cloud cloud_xoz_sort_ptr.

[0073] Step S20: Based on the ordered point cloud and according to the laser line width, an initial iterative point cloud index is set.

[0074] It is understandable that the initial iteration condition start_idx = 0. Then obtain the model iteration point cloud m_cloud_interval, specifically: according to the laser line width laser_width, set the initial iteration point cloud index start_idx, and obtain the model point cloud initial iteration point cloud m_cloud_interval through the AABB bounding box.

[0075] An AABB bounding box is a cuboid aligned with the coordinate axes that completely contains a given object or set of objects. For an object in three-dimensional space, its AABB bounding box consists of six faces, each perpendicular to the three coordinate axes. The calculation is as follows:

[0076] 1) Determine the minimum and maximum coordinate values: For a set of points (e.g., a set of vertices in a 3D model), iterate over the coordinates of all points and record the minimum and maximum coordinate values along each coordinate axis. In 2D space, find the minimum x-coordinate value (x min) and maximum x-coordinate value (x max), as well as the minimum y-coordinate value (y min) and maximum y-coordinate value (y max). In 3D space, in addition to the x- and y-coordinates, also find the minimum z-coordinate value (z min) and maximum z-coordinate value (z max).

[0077] 2) Constructing the bounding box: Based on the minimum and maximum coordinates found, construct an AABB bounding box. In two-dimensional space, an AABB bounding box is a rectangle with the lower left corner at (x min, y min) and the upper right corner at (x max, y max). In three-dimensional space, an AABB bounding box is a cuboid with one vertex at (x min, y min, z min) and the opposite vertex at (x max, y max, z max).

[0078] Step S30: Calculate the length of the AABB bounding box from the current point to the starting point to obtain a first length.

[0079] Step S40: Compare the first length with the laser line width to determine an end point index.

[0080] It should be understood that, in this embodiment, the step S40 includes: when the first length is less than the laser line width, judging whether the current point index is equal to the number of points in the ordered point cloud, and determining the end point index according to the judgment result; when the first length is greater than the laser line width, recording the end point index as the current index minus a first preset value.

[0081] Furthermore, in this embodiment, determining whether the current point index is equal to the number of points in the ordered point cloud and determining the end point index according to the determination result includes:

[0082] If the current point index is equal to the number of points in the ordered point cloud, the current index is recorded as the end point index;

[0083] If the current point index is not equal to the number of points in the ordered point cloud, the current point index is updated, and the length of the AABB bounding box of the starting point of the current point is returned, and the step of obtaining the first length is continued to iterate.

[0084] Step S50: performing iterative calculation according to the initial iterative point cloud index and the end point index to obtain an initial iterative point cloud.

[0085] It should be noted that the point cloud has been transferred to the xOz plane, and the x has been arranged in order. Therefore, to obtain the x-direction length of the AABB bounding box, it is only necessary to calculate the x distance between the two points, that is, the AABB bounding box length dist from the point start_pt(x1,0,z1) to the point end_pt(x2,0,z2) is calculated as follows:

[0086] dist = x2 - x1;

[0087] Therefore, according to the laser line width laser_width, the initial iterative point cloud m_cloud_interval flow chart is shown in 4. Figure 4 It is a flowchart for obtaining the model iteration point cloud m_cloud_interval. The specific process is as follows:

[0088] Step s501: Input the original model point cloud cloud_xoz_sort_ptr.

[0089] Step s502: Input the initial iteration point cloud index start_idx.

[0090] Step s503 , input the laser line width laser_width.

[0091] Step s504: Input the initial iteration point cloud index start_idx, record the initial iteration point as start_pt, and mark its x coordinate as pt_pre_x.

[0092] Step s505 : Initialize the current point index to cur_idx in the direction of increasing index, that is, cur_idx=start_idx+1, and record the current point as cur_pt, and its x coordinate as pt_cur_x.

[0093] Step s506: Calculate the length cur_dist of the AABB bounding box from the current point to cur_pt and the starting point start_pt.

[0094] Step s507: Compare cur_dist with the laser line width laser_width to obtain the end point index end_idx.

[0095] A. When cur_dist is less than laser_width, determine whether the cur_idx index is equal to the number of points in the original model point cloud cloud_xoz_sort_ptr. If it is equal, record the index of end_pt end_idx = cur_idx; if not,

[0096] Then update the current point index, that is, cur_idx=cur_idx+1, and return to step s506.

[0097] When cur_dist is greater than laser_width, the index of end_pt is recorded as end_idx=cur_idx-1;

[0098] Step s508: Obtain the initial iteration point cloud m_cloud_interval of the model point cloud based on the initial iteration point cloud index start_idx and the final iteration point cloud index end_idx. That is, add the point with index pt_idex∈[start_idx, end_idx] in the model point cloud cloud_xoz_sort_ptr to the m_cloud_interval point cloud.

[0099] Step S60: According to the laser focal depth and the upper limit of the processing angle, the initial iterative point cloud is corrected using the OBB bounding box to obtain the initial trajectory points and the initial trajectory angle.

[0100] In the specific implementation, the trajectory points and trajectory angles are obtained, that is, the fragmentation results. Based on the laser focal depth laser_depth, the upper limit of the cleaning angle angle_thr, the OBB bounding box is used to calculate the bounding box width, and the initial fragmentation point cloud m_cloud_interval is corrected to obtain the initial trajectory point MassCenter and the initial trajectory angle angle, and the ending index interval_end_idx of the initial iteration point cloud.

[0101] An Oriented Bounding Box (OBB) is a technology used to represent the spatial extent of a three-dimensional object. It is essentially a cuboid that fits the object closest to the object, except that the cuboid can be rotated arbitrarily according to the first-order moment of the object, that is, it is aligned with the rotation direction of the object, rather than always aligned with the coordinate axis like an Axis-Aligned Bounding Box (AABB).

[0102] Therefore, the OBB bounding box can be used to correct the initial segmented point cloud m_cloud_interval while satisfying the laser focal depth, and obtain the ending index interval_end_idx of the initial iterative point cloud. The flow chart is as follows Figure 5 As shown, Figure 5 To obtain the sharding result flow chart, the specific process is as follows:

[0103] Step s601: Input the initial iterative point cloud m_cloud_interval.

[0104] Step s602: Input the laser focal depth laser_depth and the laser line width laser_width.

[0105] Step s603: input the upper limit of the cleaning angle angle_thr.

[0106] Step s604: Input the iteration step size step. The iteration step size is related to the sparsity of the data and is generally set to 1 to 5. The larger the iteration step size, the faster the calculation speed.

[0107] Step s605: Initialize the iteration condition start_idx_of_out_range.

[0108] start_idx_of_out_range=m_cloud_interval->size()-1.

[0109] Step s606: Determine whether start_idx_of_out_range is greater than 0.

[0110] If it is greater than 0, execute step s607.

[0111] Otherwise, end the iteration.

[0112] Step s607: Calculate the OBB bounding box of the initial iterative point cloud m_cloud_interval.

[0113] Based on the OBB principle, the bounding box pose RotationMatrixObj2Cam and the bounding box center MassCenter are calculated. RotationMatrixObj2Cam is an Eigen::Matrix3f 3x3 matrix, and MassCenter is an Eigen::Vector3f vector.

[0114] Step s608: Calculate the laser cleaning angle angle.

[0115] It should be understood that the laser processing angle is the angle between the y direction of the bounding box and the z axis of the coordinate system.

[0116] It should be noted that the laser cleans along the length of the OBB bounding box. Therefore, the line width direction of the laser is aligned with the length direction of the bounding box. The cleaning angle of the laser is the angle between the y direction of the bounding box and the z axis of the coordinate system. The calculation formula is as follows:

[0117] Take the x-direction vector of the RotationMatrixObj2Cam matrix, recorded as x_dir_obj, then:

[0118] x_dir_obj=RotationMatrixObj2Cam.col(0);

[0119] Then the laser cleaning angle angle = -atan2(x_dir_obj(2),x_dir_obj(0));

[0120] Step s609: Calculate the length and width of the bounding box in the current position.

[0121] Transfer the point cloud m_cloud_interval to the coordinate system of the OBB bounding box to obtain the point cloud m_cloud_interval_trans.

[0122] 1) From step s607, we know that the pose of the bounding box is trans;

[0123]

[0124] 2) Calculate the inverse matrix of tans, denoted as Trans_inverse;

[0125]

[0126] Let the coordinates of a point on the point cloud before transformation be pt(x, y, z), then the calculation formula of the point pt_trans after transformation is as follows:

[0127] pt_trans=RotationMatrixObj2Cam_new*pt+MassCenter_new

[0128] Calculate the minimum point min_pt(x_min, 0, z_min) and the maximum point max_pt(x_max, 0, z_max) under the point cloud m_cloud_interval_trans.

[0129] x_min is the minimum x-coordinate value of all points in m_cloud_interval_trans.

[0130] z_min is the minimum z coordinate of all points in m_cloud_interval_trans.

[0131] x_max is the maximum x coordinate of all points in m_cloud_interval_trans.

[0132] z_max is the maximum value of the z coordinates of all points in m_cloud_interval_trans.

[0133] Calculate the length and width of the bounding box at the current pose.

[0134] length = x_max - x_min;

[0135] width = z_max - z_min;

[0136] Furthermore, in this embodiment, calculating the length and width of the bounding box at the current posture includes:

[0137] The ordered point cloud is converted to the current posture, and the minimum and maximum points of the point cloud at the current posture are calculated; based on the minimum and maximum points of the point cloud at the current posture, the length and width of the bounding box at the current posture are calculated.

[0138] It should be noted that the bounding box under the current pose is the bounding box of the specified pose. The pose has been specified and there is no need to regenerate the pose. Use the current pose to recalculate the minimum and maximum points. The calculation method is as follows:

[0139] Transfer the point cloud to the new bounding box pose and obtain the minimum point min_pt(x_min, 0, z_min) and the maximum point max_pt(x_max, 0, z_max) of the point cloud.

[0140] length = x_max - x_min;

[0141] width = z_max - z_min;

[0142] Step s610: Determine whether angle is greater than angle_thr.

[0143] If it is greater than angle_thr, use angle_thr as the laser cleaning angle and the bounding box pose. Using the new pose, calculate the width and length of the bounding box.

[0144] The laser cleans along the length direction of the OBB bounding box. Therefore, the length direction of the bounding box is aligned with the line width direction of the laser. That is, the posture of the bounding box at this time is rotated by angle_thr around the Y axis of the coordinate system of the initial iterative point cloud m_cloud_interval.

[0145]

[0146] Using the new pose, calculate the width and length of the bounding box. For specific methods, refer to step s609.

[0147] If it is less than angle_thr, proceed directly to step s611.

[0148] Step s611: Recalibrate MassCenter in the XOZ plane.

[0149] Since step s610 may have updated the bounding box's pose matrix RotationMatrixObj2Cam but not MassCenter, MassCenter may not be the center of the bounding box. Therefore, MassCenter needs to be corrected to the center of the bounding box. The specific calculation method is as follows:

[0150] Transfer the point cloud to the new bounding box pose and obtain the minimum point min_pt(x_min, 0, z_min) and the maximum point max_pt(x_max, 0, z_max) of the point cloud.

[0151] The calculation formula for the center coordinate p_MassCenter(x_new, 0, z_new) in the bounding box coordinate system is as follows:

[0152] x_new = (x_min + x_max) / 2.0;

[0153] z_max = (z_min + z_max) / 2.0;

[0154] Transfer p_MassCenter to the point cloud coordinate system to get the new MassCenter, that is,

[0155] MassCenter=RotationMatrixObj2Cam*p_MassCenter+MassCenter;

[0156] Step s612: Determine whether the length of the bounding box is less than the laser line width laser_width, and whether the width of the bounding box is less than the laser focal depth laser_depth.

[0157] If not, use step to update the initial iterative point cloud m_cloud_interval, that is, remove the last iteration step of step points of the m_cloud_interval point cloud to obtain a new point cloud, update the iteration condition start_idx_of_out_range = end_idx = m_cloud_ptr->size()-1-step, and return to step s506.

[0158] If satisfied, the iteration ends and the trajectory point MassCenter and trajectory angle angle are output. At this time, the output is the initial trajectory point and initial trajectory angle.

[0159] The fragmentation results are usually stored, that is, the initial trajectory points and initial trajectory angles are output and stored in the results.

[0160] Furthermore, in this embodiment, step S60 includes:

[0161] Calculating the OBB bounding box of the initial iterative point cloud;

[0162] Calculate the laser processing angle based on the OBB bounding box;

[0163] Calculate the length and width of the bounding box under the current pose;

[0164] Determining whether the laser processing angle is greater than the processing angle upper limit;

[0165] If the laser processing angle is less than the processing angle upper limit, recalibrate the center of the bounding box;

[0166] If the laser processing angle is greater than the processing angle upper limit, the processing angle upper limit is used as the current processing angle, a new bounding box pose is obtained, and the process returns to the step of calculating the length and width of the bounding box at the current pose;

[0167] Determine whether the length of the bounding box in the current posture is less than the laser line width, and whether the width of the bounding box in the current posture is less than the laser focal depth;

[0168] If not, the initial iterative point cloud is updated according to the iteration step to obtain a new point cloud, the iteration condition is updated, and the length of the AABB bounding box of the starting point of the current point is returned to continue the iteration by obtaining the first length.

[0169] If satisfied, the iteration ends and the initial trajectory point and initial trajectory angle of the workpiece to be processed are output.

[0170] Step S70: updating the iteration condition, and determining whether the updated iteration condition is less than the number of points in the model point cloud.

[0171] Step S80: If it is not less than, then end the iterative calculation and obtain the target trajectory point and target trajectory angle.

[0172] It is understandable that the iteration condition start_idx is updated to be equal to start_idx + interval_end_idx. It is determined whether the iteration condition start_idx is less than the number of points in the cloud_xoz_ptr point cloud.

[0173] If start_idx is less than the number of points in the cloud_xoz_ptr point cloud, the process returns to step S50 and iterates the calculation again.

[0174] If start_idx is not less than the number of points in the cloud_xoz_ptr point cloud, the iterative settlement ends and the result is output.

[0175] It should be noted that the final results are as follows Figure 6 As shown, the target trajectory point and target trajectory angle are output.

[0176] In this embodiment, a model point cloud of the workpiece to be processed is obtained, and the model point cloud is sorted to obtain an ordered point cloud. Based on the ordered point cloud, an initial iterative point cloud index is set according to the laser line width. The length of the AABB bounding box from the current point to the starting point is calculated to obtain a first length. The first length is compared with the laser line width to determine the end point index. An iterative calculation is performed based on the initial iterative point cloud index and the end point index to obtain an initial iterative point cloud. Based on the laser focal depth and the upper limit of the processing angle, the initial iterative point cloud is corrected using the OBB bounding box to obtain the initial trajectory point and initial trajectory angle. The iteration condition is updated to determine whether the updated iteration condition is less than the number of points in the model point cloud. If not, the iterative calculation is terminated to obtain the target trajectory point and target trajectory angle. Based on the limited laser focal depth and width, as well as the robot angle limit during processing, an effective envelope is generated. The target trajectory can be generated based on the envelope, which can effectively improve the operation accuracy and efficiency.

[0177] In addition, an embodiment of the present invention further proposes a storage medium, on which a program for generating a trajectory based on focal depth and width constraints is stored. When the program for generating a trajectory based on focal depth and width constraints is executed by a processor, the steps of the method for generating a trajectory based on focal depth and width constraints as described above are implemented.

[0178] In addition, refer to Figure 7 The embodiment of the present invention further provides a trajectory generation device based on focal depth and width constraints, the trajectory generation device based on focal depth and width constraints comprising:

[0179] A sorting module 10 is used to obtain a model point cloud of a workpiece to be processed, sort the model point cloud, and obtain an ordered point cloud;

[0180] A setting module 20 is used to set an initial iterative point cloud index based on the ordered point cloud and according to the laser line width;

[0181] A calculation module 30 is used to calculate the length of the AABB bounding box from the current point to the starting point to obtain a first length;

[0182] A comparison module 40 is configured to compare the first length with the laser line width to determine an end point index;

[0183] The calculation module 30 is further configured to perform iterative calculation based on the initial iterative point cloud index and the end point index to obtain an initial iterative point cloud;

[0184] A correction module 50 is used to correct the initial iterative point cloud using an OBB bounding box according to the laser focal depth and the upper limit of the processing angle to obtain initial trajectory points and initial trajectory angles;

[0185] A judgment module 60 is used to update the iteration condition and judge whether the updated iteration condition is less than the number of points in the model point cloud;

[0186] The calculation module 30 is further configured to terminate the iterative calculation if is not less than , and obtain the target trajectory point and the target trajectory angle.

[0187] Other embodiments or specific implementations of the device for generating a trajectory based on depth of focus and width constraints of the present invention may refer to the above-mentioned method embodiments and will not be described in detail here.

[0188] It should be noted that, in this document, the terms "comprises," "includes," or any other variations thereof are intended to encompass non-exclusive inclusion, such that a process, method, article, or system comprising a series of elements includes not only those elements but also other elements not explicitly listed, or elements inherent to such process, method, article, or system. In the absence of further limitations, an element defined by the phrase "comprising a ..." does not exclude the presence of other identical elements in the process, method, article, or system comprising the element.

[0189] The serial numbers of the embodiments of the present invention are for descriptive purposes only and do not represent superiority or inferiority of the embodiments. In a unit claim that lists several means, several of these means may be embodied by the same item of hardware. The use of the terms first, second, and third, etc., does not denote any order and should be construed as identifiers.

[0190] Through the description of the above embodiments, those skilled in the art can clearly understand that the above embodiment methods can be implemented by means of software plus the necessary general hardware platform, or of course by hardware, but in many cases the former is a better embodiment. Based on this understanding, the technical solution of the present invention is essentially or the part that contributes to the prior art can be embodied in the form of a software product, which is stored in a storage medium (such as a read-only memory image (ROM) / random access memory (RAM), a magnetic disk, or an optical disk), and includes a number of instructions for enabling a terminal device (which can be a mobile phone, a computer, a server, an air conditioner, or a network device, etc.) to execute the methods described in each embodiment of the present invention.

[0191] The above are only preferred embodiments of the present invention and are not intended to limit the patent scope of the present invention. Any equivalent structure or equivalent process transformation made using the contents of the present invention description and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. A trajectory generation method based on focal depth and width constraints, characterized in that: The method for generating a trajectory based on focal depth and width constraints comprises the following steps: Acquire a model point cloud of a workpiece to be processed, and sort the model point cloud to obtain an ordered point cloud; Based on the ordered point cloud, an initial iterative point cloud index is set according to the laser line width; Calculate the length of the AABB bounding box from the current point to the starting point to obtain the first length; comparing the first length with the laser line width to determine an end point index; Performing iterative calculation according to the initial iterative point cloud index and the end point index to obtain an initial iterative point cloud; According to the laser focal depth and the upper limit of the processing angle, the initial iterative point cloud is corrected using the OBB bounding box to obtain the initial trajectory point and the initial trajectory angle; Updating the iteration condition, and determining whether the updated iteration condition is less than the number of points in the model point cloud; If it is not less than, the iterative calculation ends and the target trajectory point and target trajectory angle are obtained.

2. The method for generating a trajectory based on focal depth and width constraints according to claim 1, wherein: Comparing the first length with the laser line width to determine an end point index includes: When the first length is less than the laser line width, determining whether the current point index is equal to the number of points in the ordered point cloud, and determining the end point index according to the determination result; When the first length is greater than the laser line width, the recording end point index is the current index minus a first preset value.

3. The method for generating a trajectory based on focal depth and width constraints according to claim 2, wherein: Determining whether the current point index is equal to the number of points in the ordered point cloud and confirming the end point index according to the determination result includes: If the current point index is equal to the number of points in the ordered point cloud, the current index is recorded as the end point index; If the current point index is not equal to the number of points in the ordered point cloud, the current point index is updated, and the length of the AABB bounding box of the starting point of the current point is returned, and the step of obtaining the first length is continued to iterate.

4. The method for generating a trajectory based on focal depth and width constraints according to any one of claims 1 to 3, wherein: According to the laser focal depth and the upper limit of the processing angle, the initial iterative point cloud is corrected using the OBB bounding box to obtain the initial trajectory points and initial trajectory angles, including: Calculating the OBB bounding box of the initial iterative point cloud; Calculate the laser processing angle based on the OBB bounding box; Calculate the length and width of the bounding box under the current pose; Determining whether the laser processing angle is greater than the processing angle upper limit; If the laser processing angle is less than the processing angle upper limit, recalibrate the center of the bounding box; Determine whether the length of the bounding box in the current posture is less than the laser line width, and whether the width of the bounding box in the current posture is less than the laser focal depth; If not, the initial iterative point cloud is updated according to the iteration step to obtain a new point cloud, the iteration condition is updated, and the length of the AABB bounding box of the starting point of the current point is returned to continue the iteration by obtaining the first length. If satisfied, the iteration ends and the initial trajectory point and initial trajectory angle of the workpiece to be processed are output.

5. The method for generating a trajectory based on focal depth and width constraints according to claim 4, wherein: After the step of determining whether the laser processing angle is greater than the processing angle upper limit, the method further includes: If the laser processing angle is greater than the processing angle upper limit, the processing angle upper limit is used as the current processing angle, a new bounding box pose is obtained, and the process returns to the step of calculating the length and width of the bounding box at the current pose.

6. The method for generating a trajectory based on focal depth and width constraints according to claim 4, wherein: Calculate the length and width of the bounding box at the current pose, including: Convert the ordered point cloud to the current pose, and calculate the minimum and maximum points of the point cloud in the current pose; Calculate the length and width of the bounding box at the current pose based on the minimum and maximum points of the point cloud at the current pose.

7. The method for generating a trajectory based on focal depth and width constraints according to claim 4, wherein: The laser processing angle is the angle between the y direction of the bounding box and the z axis of the coordinate system.

8. A trajectory generation device based on focal depth and width constraints, characterized in that: The trajectory generation device based on focal depth and width constraints includes: A sorting module is used to obtain a model point cloud of a workpiece to be processed, sort the model point cloud, and obtain an ordered point cloud; A setting module, configured to set an initial iterative point cloud index based on the ordered point cloud and according to the laser line width; A calculation module, used to calculate the length of the AABB bounding box from the current point to the starting point to obtain a first length; a comparison module, configured to compare the first length with the laser line width to determine an end point index; The calculation module is further configured to perform iterative calculation based on the initial iterative point cloud index and the end point index to obtain an initial iterative point cloud; A correction module is used to correct the initial iterative point cloud using an OBB bounding box according to the laser focal depth and the upper limit of the processing angle to obtain an initial trajectory point and an initial trajectory angle; A judgment module, used for updating the iteration condition and judging whether the updated iteration condition is less than the number of points in the model point cloud; The calculation module is further configured to terminate the iterative calculation if is not less than and obtain the target trajectory point and the target trajectory angle.

9. A trajectory generation device based on focal depth and width constraints, characterized in that: The device for generating a trajectory based on focal depth and width constraints includes: a memory, a processor, and a program for generating a trajectory based on focal depth and width constraints stored in the memory and executable on the processor. When the program for generating a trajectory based on focal depth and width constraints is executed by the processor, the steps of the method for generating a trajectory based on focal depth and width constraints as described in any one of claims 1 to 7 are implemented.

10. A storage medium, characterized in that: The storage medium stores a trajectory generation program based on focal depth and width constraints. When the trajectory generation program based on focal depth and width constraints is executed by a processor, the steps of the trajectory generation method based on focal depth and width constraints according to any one of claims 1 to 7 are implemented.

Images