A path planning method and device, electronic equipment and storage medium

Abstract

The application provides a path planning method and device, electronic equipment and storage medium, the method comprises the following steps: obtaining the surface point cloud data of the target object in the task through the point cloud acquisition device, converting the surface point cloud data into general point cloud data, defining the contact surface of the tool to obtain the target tool contact plane and plane data, performing path interpolation on the task points based on the target tool contact plane, obtaining the path key points, for each path key point in the path key point queue, adjusting the tool pose based on the target point cloud in the preset iteration round, converting the plane data of the target tool contact plane based on the tool pose, calculating the weighted distance sum of squares of the target point cloud to the target tool contact plane based on the converted plane data, when the weighted distance sum of squares is less than a specified threshold or the iteration round reaches a maximum round, constructing the tool pose vector of the path key point, and after calculating all the path key points, obtaining the smooth tool pose path.

Description

Technical Field

[0001] This application relates to the field of physiotherapy technology, and more specifically, to a path planning method, apparatus, electronic device, and storage medium. Background Technology

[0002] In the fields of robotic automation and CNC machining, pose planning aims to accurately determine the target position and orientation of the end effector or workpiece in Cartesian space (i.e., six-degree-of-freedom pose parameters).

[0003] Currently, most methods involve single-tool pose planning or path obstacle avoidance planning, as well as the commonly used single-plane fitting calculation method. Conventional pose planning typically uses a planar approximation tool contact surface to work on the object surface.

[0004] However, conventional pose planning is only suitable for single-contact tools working on relatively flat surfaces. It has poor versatility and is difficult to apply in task environments with significant undulations in multiple contact surfaces, such as multi-contact physiotherapy tools that require conforming to the human body. Summary of the Invention

[0005] In view of this, the purpose of this application is to provide a path planning method, apparatus, electronic device and storage medium, which calculates the tool pose vector of the path key points by using the point cloud of the surface of the task target object and the contact plane data of the multi-plane circular approximation target tool defined by the user, and generates the tool pose path after interpolation and smoothing of all path key points. This can be applied in task environments with obvious undulations of multiple contact surfaces of the tool, thus improving the versatility of path planning.

[0006] In a first aspect, embodiments of this application provide a path planning method, the method comprising: The surface point cloud data of the target object in the task is acquired through a preset point cloud acquisition device, and the surface point cloud data is converted into general point cloud data. Define the contact surface of the tool to obtain the planar data of the contact plane of the target tool and the contact plane of the target tool, and perform path interpolation on the task points based on the contact plane of the target tool to obtain the path key points; Each path key point is pushed into the path key point queue to be processed in sequence. For each path key point in the path key point queue, the target point cloud is obtained, and the tool pose is adjusted based on the target point cloud in a preset iteration cycle. Based on the tool pose, the planar data of the target tool contact plane is transformed, and the weighted sum of squared distances from the target point cloud to the target tool contact plane is calculated based on the transformed planar data; When the weighted sum of squared distances is less than a specified threshold or the number of iterations reaches the maximum number of iterations, the tool pose vectors of the path key points are constructed, and after calculating the tool pose vectors of all path key points, a smooth tool pose path is obtained based on the tool pose vectors of all path key points.

[0007] In one possible implementation, defining the contact surface of the tool, obtaining planar data of the target tool contact plane and the target tool contact plane, includes: Determine the tool installation center point and define the tool installation center point as the origin of its own tool coordinate system; In the self-tool coordinate system, with the tool installation center point as the origin, construct the center coordinates of the self-tool coordinate system, the plane normal vector pointing to the origin, and the radius of the circle to obtain a planar circle; Based on the contact surface of the planar circular covering tool in actual application, the contact plane of the target tool and the planar data of the contact plane of the target tool are obtained.

[0008] In one possible implementation, the step of interpolating the path based on the target tool contact plane to obtain path key points includes: Determine the starting point coordinates and ending point coordinates of the task, and calculate the distance between the starting point and ending point based on the vector difference between the starting point coordinates and the ending point coordinates; Under the preset coordinate difference interpolation method, the ratio of the distance from the current path key point to the starting point coordinate to the starting point distance is obtained based on the starting point distance and the preset interpolation step size. Based on the ratio, the starting point coordinates, and the ending point coordinates, the position coordinates of each key point on the path are calculated sequentially according to the ratio.

[0009] In one possible implementation, acquiring the target point cloud and adjusting the tool pose based on the target point cloud in a preset number of iterations includes: A spatial search tree is constructed based on the general point cloud data. The target point cloud is then searched using the target point radius search method of the spatial search tree to find all points within the location coordinates of the path key points and the preset tool radius. The preset tool radius is determined based on the radius of the activity range corresponding to each contact surface of the tool. The tool pose is adjusted according to the current gradient descent direction with a preset number of iterations and a corresponding adjustment step size to obtain the adjusted tool pose; wherein the adjustment step size gradually decreases as the number of iterations increases.

[0010] In one possible implementation, the step of converting the planar data of the target tool contact plane based on the tool pose, and calculating the weighted sum of squared distances from the target point cloud to the target tool contact plane based on the converted planar data, includes: The center coordinates and plane normal vector of the target tool contact plane are multiplied by the tool pose to obtain the transformed center coordinates and plane normal vector. Points within the radius of a circle on each target tool's contact plane are selected to form a planar circular neighbor point cloud. Based on the coordinates of each point in the planar circular neighbor point cloud, combined with the transformed center coordinates and the plane normal vector, the target distance from each point to the corresponding target tool's contact plane is calculated. Here, the circular radius represents the circular radius of each tool contact plane of the tool. Based on the target distance, the sum of squared distances from each point in the planar circular neighbor point cloud to the target tool contact plane is calculated. Based on the sum of squared distances and the planar circular weights of the corresponding target tool contact plane, the weighted sum of squared distances from the target point cloud to the target tool contact plane is calculated.

[0011] In one possible implementation, the tool pose vector for constructing the path key points includes: Determine the Euler angle data of the tool pose obtained from iterative calculation; The Euler angle data and the position coordinates of the path key points are merged to generate the tool pose vector of the path key points.

[0012] In one possible implementation, obtaining a smooth tool pose path based on the tool pose vectors of all path key points includes: The path key points are densely interpolated using a preset path pose interpolation method to generate the original path points; The original path points are filtered, and a smooth tool pose path is output based on the tool pose vector corresponding to the filtered original path points.

[0013] Secondly, embodiments of this application also provide a path planning device, the device comprising: The conversion module is used to acquire surface point cloud data of the target object in the task through a preset point cloud acquisition device, and convert the surface point cloud data into general point cloud data. The first acquisition module is used to define the contact surface of the tool to obtain the planar data of the contact plane of the target tool and the contact plane of the target tool, and to perform path interpolation on the task points based on the contact plane of the target tool to obtain the path key points; The adjustment module is used to push each path key point into the path key point queue to be processed in sequence, and for each path key point in the path key point queue, obtain the target point cloud, and adjust the tool pose based on the target point cloud in a preset iteration cycle. The calculation module is used to convert the planar data of the target tool contact plane based on the tool pose, and calculate the weighted sum of squared distances from the target point cloud to the target tool contact plane based on the converted planar data; The second acquisition module is used to construct the tool pose vector of the path key points when the weighted sum of squared distances is less than a specified threshold or the iteration round reaches the maximum round, and to obtain a smooth tool pose path based on the tool pose vector of all path key points after calculating the tool pose vector of all path key points.

[0014] In one possible implementation, the first acquisition module is specifically used for: Determine the tool installation center point and define the tool installation center point as the origin of its own tool coordinate system; In the self-tool coordinate system, with the tool installation center point as the origin, construct the center coordinates of the self-tool coordinate system, the plane normal vector pointing to the origin, and the radius of the circle to obtain a planar circle; Based on the contact surface of the planar circular covering tool in actual application, the contact plane of the target tool and the planar data of the contact plane of the target tool are obtained.

[0015] In one possible implementation, the first acquisition module is specifically used for: Determine the starting point coordinates and ending point coordinates of the task, and calculate the distance between the starting point and ending point based on the vector difference between the starting point coordinates and the ending point coordinates; Under the preset coordinate difference interpolation method, the ratio of the distance from the current path key point to the starting point coordinate to the starting point distance is obtained based on the starting point distance and the preset interpolation step size. Based on the ratio, the starting point coordinates, and the ending point coordinates, the position coordinates of each key point on the path are calculated sequentially according to the ratio.

[0016] In one possible implementation, the adjustment module is specifically used for: A spatial search tree is constructed based on the general point cloud data. The target point cloud is then searched using the target point radius search method of the spatial search tree to find all points within the location coordinates of the path key points and the preset tool radius. The preset tool radius is determined based on the radius of the activity range corresponding to each contact surface of the tool. The tool pose is adjusted according to the current gradient descent direction with a preset number of iterations and a corresponding adjustment step size to obtain the adjusted tool pose; wherein the adjustment step size gradually decreases as the number of iterations increases.

[0017] In one possible implementation, the computing module is specifically used for: The center coordinates and plane normal vector of the target tool contact plane are multiplied by the tool pose to obtain the transformed center coordinates and plane normal vector. Points within the radius of a circle on each target tool's contact plane are selected to form a planar circular neighbor point cloud. Based on the coordinates of each point in the planar circular neighbor point cloud, combined with the transformed center coordinates and the plane normal vector, the target distance from each point to the corresponding target tool's contact plane is calculated. Here, the circular radius represents the circular radius of each tool contact plane of the tool. Based on the target distance, the sum of squared distances from each point in the planar circular neighbor point cloud to the target tool contact plane is calculated. Based on the sum of squared distances and the planar circular weights of the corresponding target tool contact plane, the weighted sum of squared distances from the target point cloud to the target tool contact plane is calculated.

[0018] In one possible implementation, the second acquisition module is specifically used for: Determine the Euler angle data of the tool pose obtained from iterative calculation; The Euler angle data and the position coordinates of the path key points are merged to generate the tool pose vector of the path key points.

[0019] In one possible implementation, the second acquisition module is specifically used for: The path key points are densely interpolated using a preset path pose interpolation method to generate the original path points; The original path points are filtered, and a smooth tool pose path is output based on the tool pose vector corresponding to the filtered original path points.

[0020] Thirdly, embodiments of this application provide an electronic device, including: a processor, a storage medium, and a bus, wherein the storage medium stores machine-readable instructions executable by the processor, and when the electronic device is running, the processor communicates with the storage medium via the bus, and the processor executes the machine-readable instructions to perform the steps of the path planning method as described in any of the first aspects.

[0021] Fourthly, embodiments of this application provide a computer-readable storage medium storing a computer program, which, when executed by a processor, performs the steps of the path planning method described in any of the first aspects.

[0022] This application provides a path planning method, apparatus, electronic device, and storage medium. It acquires surface point cloud data of a target object in a task using a preset point cloud acquisition device, converts the surface point cloud data into general point cloud data, defines the contact surface of a tool to obtain planar data of the target tool contact plane, and performs path interpolation on task points based on the target tool contact plane to obtain path key points. Each path key point is sequentially pushed into a queue of path key points to be processed. For each path key point in the queue, a target point cloud is acquired, and the tool pose is adjusted based on the target point cloud under a preset number of iterations. The planar data of the target tool contact plane is converted based on the tool pose. The weighted sum of squared distances from the target point cloud to the target tool contact plane is calculated based on the converted planar data. When the weighted sum of squared distances is less than a specified threshold or the number of iterations reaches the maximum, a tool pose vector for the path key points is constructed. After calculating the tool pose vectors for all path key points, a smooth tool pose path is obtained based on the tool pose vectors of all path key points. This application calculates the tool pose vector of key path points by using the point cloud of the target object surface and the contact plane data of the multi-plane circular approximation target tool defined by the user, and then generates the tool pose path by interpolating and smoothing all key path points. This allows it to be applied in task environments with significant undulations in multiple contact surfaces of the tool, thus improving the versatility of path planning.

[0023] To make the above-mentioned objectives, features and advantages of this application more apparent and understandable, preferred embodiments are described below in detail with reference to the accompanying drawings. Attached Figure Description

[0024] To more clearly illustrate the technical solutions of the embodiments of this application, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of this application and should not be regarded as a limitation of the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.

[0025] Figure 1 This is a flowchart of the path planning method provided according to the embodiments of this application; Figure 2 This is a schematic diagram of the overall path planning process; Figure 3This is a schematic diagram of the path planning device provided according to an embodiment of this application; Figure 4 This is a schematic diagram of the structure of an electronic device provided according to an embodiment of this application. Detailed Implementation

[0026] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. It should be understood that the accompanying drawings in this application are for illustrative and descriptive purposes only and are not intended to limit the scope of protection of this application. Furthermore, it should be understood that the schematic drawings are not drawn to scale. The flowcharts used in this application illustrate operations implemented according to some embodiments of this application. It should be understood that the operations in the flowcharts may not be implemented in sequence, and steps without logical contextual relationships may be reversed or implemented simultaneously. In addition, those skilled in the art, guided by the content of this application, may add one or more other operations to the flowcharts, or remove one or more operations from the flowcharts.

[0027] Furthermore, the described embodiments are merely some, not all, of the embodiments of this application. The components of the embodiments of this application described and illustrated herein can typically be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of this application provided in the accompanying drawings is not intended to limit the scope of the claimed application, but merely to illustrate selected embodiments of the application. All other embodiments obtained by those skilled in the art based on the embodiments of this application without inventive effort are within the scope of protection of this application.

[0028] It should be noted that the term "comprising" will be used in the embodiments of this application to indicate the presence of the features declared thereafter, but does not exclude the addition of other features.

[0029] In the fields of robotic automation and CNC machining, pose planning aims to accurately determine the target position and orientation of the end effector or workpiece in Cartesian space (i.e., six-degree-of-freedom pose parameters).

[0030] Currently, most methods involve single-tool pose planning or path obstacle avoidance planning, as well as the commonly used single-plane fitting calculation method. Conventional pose planning typically uses a single plane to approximate the tool's contact surface on the object's surface. However, conventional pose planning is only suitable for tools with a single contact surface working on relatively flat surfaces, resulting in poor versatility and difficulty in application to task environments with significant undulations and multiple contact surfaces, such as scenarios requiring multi-contact surface physiotherapy tools that conform to the human body.

[0031] To address this problem, this application provides a path planning method, apparatus, electronic device, and storage medium. By using the point cloud of the target object's surface and the contact plane data of a manually defined multi-plane circular approximation target tool, the tool pose vector of the path key points is calculated using the principle of iterative gradient approximation. After interpolating and smoothing all path key points, the tool pose path is generated. This allows the method to be applied in task environments with significant undulations in multiple contact surfaces of the tool, thus improving the versatility of path planning.

[0032] Figure 1 This is a flowchart of a path planning method provided according to an embodiment of this application. For example... Figure 1 As shown, the path planning method in this application embodiment may specifically include: S101. Acquire surface point cloud data of the target object in the task through a preset point cloud acquisition device, and convert the surface point cloud data into general point cloud data.

[0033] S102. Define the contact surface of the tool to obtain the planar data of the contact plane of the target tool and the contact plane of the target tool, and perform path interpolation on the task points based on the contact plane of the target tool to obtain the path key points.

[0034] S103. Push each path key point into the path key point queue to be processed in sequence. For each path key point in the path key point queue, obtain the target point cloud and adjust the tool pose based on the target point cloud in the preset iteration round.

[0035] S104. Based on the tool pose, transform the planar data of the target tool contact plane, and calculate the weighted sum of squared distances from the target point cloud to the target tool contact plane based on the transformed planar data.

[0036] S105. When the weighted sum of squared distances is less than a specified threshold or the number of iterations reaches the maximum, construct the tool pose vectors of the path key points, and after calculating the tool pose vectors of all path key points, obtain a smooth tool pose path based on the tool pose vectors of all path key points.

[0037] In the above path planning method, the tool pose vector of the path key points is calculated by using the point cloud of the surface of the task target object and the contact plane data of the multi-plane circular approximation target tool defined by the user. The tool pose path is generated after interpolation and smoothing of all path key points. This method can be applied in task environments with obvious undulations of multiple contact surfaces of the tool, thus improving the versatility of path planning.

[0038] The exemplary steps described above in the embodiments of this application are illustrated below with specific examples: S101 acquires surface point cloud data of the target object in the task through a preset point cloud acquisition device, and converts the surface point cloud data into general point cloud data.

[0039] In this embodiment, the point cloud acquisition device includes at least a depth camera, an infrared camera, and a lidar. The device is used to acquire surface point cloud data of the target object in the task and convert it into general point cloud data (sourcePC). The data format is a set of point cloud coordinates, where the point coordinates p_pci contain x, y, and z axis data. For example... Figure 2 As shown.

[0040] S102, define the contact surface of the tool, obtain the planar data of the target tool contact plane and the target tool contact plane, and perform path interpolation on the task points based on the target tool contact plane to obtain the path key points.

[0041] Among them, the planar data is the complete tool contact plane data; the planar data contains multiple spatial circular parameters, each planar circle corresponds to a spatial circular parameter, and each spatial circular parameter is a seven-dimensional vector, including at least the center coordinates of the circle with the tool installation center point as the origin, the plane normal vector, and the circle radius.

[0042] In this embodiment, the tool refers to an object in path planning that comes into contact with the human body or a surface, such as a physiotherapy tool in a physiotherapy scenario. The target tool contact plane is an approximate contact plane of the tool. Each tool contact plane corresponds to an approximate tool contact plane. The contact plane of the tool can be digitized manually according to the actual needs of the tool to obtain the planar data (spatial coordinate data) of the target tool contact plane and the target tool contact plane. Based on the target tool contact plane, path interpolation is performed on the task points to obtain key path points for subsequent processing. For example, such as... Figure 2 As shown.

[0043] Optionally, when defining the contact surface of the tool and obtaining the planar data of the target tool contact plane and the target tool contact plane, the tool installation center point is determined and defined as the origin of the tool's own coordinate system, thus obtaining the tool's own coordinate system; under the tool's own coordinate system, with the tool installation center point as the origin, the circle center coordinates, the plane normal vector pointing to the origin, and the circle radius of the tool's own coordinate system are constructed to obtain the planar circle; based on the planar circle covering the contact surface of the tool in actual application, the planar data of the target tool contact plane and the target tool contact plane are obtained.

[0044] Specifically, the tool's installation center point is defined as its own tool coordinate origin. Within its own tool coordinate system, a planar circle covers the contact surface of the tool during actual application. Complete tool contact plane data (plane data) includes the tool coordinates (also known as the control point, located at the tool's installation center point p). tc It includes spatial circular parameters of the approximate tool contact surface (target tool contact plane), which includes three axes (x, y, z). Each spatial circular parameter is a seven-dimensional vector containing the center coordinates pi of the tool's own coordinate system (with the tool control point as the origin ptc), the plane normal vector ni pointing to the origin of the coordinate system, and the circular radius ri.

[0045] It should be noted that the starting point and ending point coordinates of the task are determined, the distance between the starting and ending points is calculated based on the vector difference between the starting and ending point coordinates, and under the preset coordinate difference interpolation method, the starting point coordinates and ending point coordinates are interpolated based on the distance between the starting and ending points and the preset interpolation step size, and the position coordinates of each key point on the path are calculated in sequence according to the proportion.

[0046] Optionally, the starting and ending coordinates of the task are determined, and the distance between the starting and ending points is calculated based on the vector difference between the starting and ending coordinates. Under a preset coordinate difference interpolation method, the ratio of the distance from the current path key point to the starting coordinate to the starting and ending point distance is obtained based on the distance between the starting and ending points and a preset interpolation step size. Based on the ratio, the starting coordinates, and the ending coordinates, the position coordinates of each path key point are calculated sequentially according to the ratio. The ratio is less than or equal to 1, and the interpolation step size is not 0.

[0047] Specifically, for example, based on the starting point coordinates startPos and ending point coordinates endPos, the required coordinate interpolation method (e.g., linear interpolation, B-spline interpolation, etc.) is used to perform equidistant coordinate interpolation with an interpolation step size s, calculate the path key points (keyPos), and push them into the queue of path key points to be processed in sequence. The formula for calculating the i-th path key point of linear interpolation is: dis_SE = |endPos - startPos|; r = i / (dis_SE / s); kpi = r * startPos + (1 - r) * endPos; Where dis_SE is the magnitude of the difference between the coordinate vectors of the start and end points, i.e. the distance between the start and end points, r is the ratio of the distance from the current path key point to the start point (the distance between the coordinates of the task start point) to the distance between the start and end points of dis_SE, and r ≤ 1, s is not zero.

[0048] It should be noted that the planar circle that the approximate tool contacts can be replaced with other types of planar graphics.

[0049] S103, push each path key point into the path key point queue to be processed in sequence, obtain the target point cloud for each path key point in the path key point queue, and adjust the tool pose based on the target point cloud in the preset iteration round.

[0050] In this embodiment of the application, for each path key point in the path key point queue in step S102, a target point cloud is obtained, and the tool pose is iteratively adjusted based on the target point cloud in each iteration round for subsequent processing. For example, as Figure 2 As shown. Optionally, when acquiring the target point cloud and adjusting the tool pose based on the target point cloud in a preset number of iterations, a spatial search tree is constructed based on the general point cloud data. The target point radius search method of the spatial search tree is used to search for the position coordinates of the key points of the path and all points within the preset tool radius as the target point cloud. The tool pose is adjusted according to the current gradient descent direction with a preset number of iterations and the corresponding adjustment step size to obtain the adjusted tool pose.

[0051] The preset tool radius is determined based on the radius of the activity range corresponding to each contact surface of the tool; the adjustment step size gradually decreases as the number of iterations increases, and only the value on one axis is adjusted in each round; the tool pose is a rotation vector, representing the direction of the rotation axis and the angle of rotation around that axis (rotation angle), and the gradient descent direction represents the direction of the rotation axis.

[0052] Specifically, the location coordinates keyPos of the first path key point to be processed are taken from the key point queue. A spatial search tree KDTree is constructed based on the general point cloud sourcePC mentioned above. The target point radius search method of KDTree is used to search for all points within the location coordinates of the path key point and the preset tool radius R_pc, which are used as the target point cloud targetPC.

[0053] Continuing, the tool pose `rotVec` is adjusted based on the current gradient descent direction `rotVecDir` (initial data is (1,1,1)). The adjustment step size is `step` (the `step` step size gradually decreases as the iteration number `iterIndex` increases). Only the value on one axis is adjusted in each iteration. The calculation formula is: rotVec += rotVecDir * step For example, the calculation rule is that if the round number iterIndex modulo 3 equals 0, then the data of the 0th axis of rotVecDir is valid.

[0054] S104, based on the tool pose, transform the planar data of the target tool contact plane, and calculate the weighted sum of squared distances from the target point cloud to the target tool contact plane based on the transformed planar data.

[0055] In this embodiment, based on the iteratively adjusted tool pose in step S103, the planar data of the target tool contact plane is converted into the target point cloud coordinate system. The weighted sum of squared distances from the target point cloud to the target tool contact plane is then calculated based on the converted planar data for subsequent processing. For example, as... Figure 2 As shown.

[0056] In some implementations, the center coordinates and plane normal vectors of the target tool contact planes are multiplied by the tool pose using a left-hand vector operation to obtain the transformed center coordinates and plane normal vectors. Points within the circular radius of each target tool contact plane are selected to form a planar circular neighbor point cloud. Based on the coordinates of each point in the planar circular neighbor point cloud, combined with the transformed center coordinates and plane normal vectors, the target distance from each point to the corresponding target tool contact plane is calculated. Based on the target distances, the sum of squared distances from each point in the planar circular neighbor point cloud to the target tool contact plane is calculated. Based on the sum of squared distances and the planar circular weights of the corresponding target tool contact planes, the weighted sum of squared distances from the target point cloud to the target tool contact plane is calculated. Here, the circular radius represents the circular radius of each tool contact plane.

[0057] Specifically, this application recalculates the approximate spatial position of the tool contact plane, that is, transforms the spatial coordinate data of the tool contact plane (i.e., plane data) to the target point cloud coordinate system, which also involves transforming the center coordinates pi and the plane normal vector ni. The calculation formula is as follows: pNewi = rotVec * pi; nNewi = rotVec * ni; Here, a special Euler angle left-multiply vector method is used, which includes the transformation from Euler angles to rotation matrix and the left-multiply vector coordinates of rotation matrix.

[0058] Continuing, points within the circular radius r_maxDis of the contact plane of each target tool are selected to form a circular neighbor point cloud plane_neighborPC. The weighted sum of squared distances wdss from the point cloud to the contact plane of the tool's current pose is calculated using the following formula: di = nNewi* pointi pointj- nNewi*nNewi dssi = Σ(di) 2 wdss = q0*dss0+...+qn-1*dssn-1 Where, di is the distance from the point cloud point to the i-th planar circle (the plane in contact with the target tool), nNewi is the transformed plane normal vector, pointi is the coordinate of the i-th point in the neighbor point cloud plane_neighborPC of the planar circle, pointj is the coordinate of the j-th point in the neighbor point cloud plane_neighborPC of the planar circle; dssi is the sum of squared distances from the i-th neighbor point cloud point to the plane containing the planar circle; q0 to qn-1 are the planar circle weights of the n planar circles, which can be manually adjusted according to the actual effect.

[0059] S105, when the weighted sum of squared distances is less than a specified threshold or the number of iterations reaches the maximum number of iterations, construct the tool pose vectors of the path key points, and after calculating the tool pose vectors of all path key points, obtain a smooth tool pose path based on the tool pose vectors of all path key points.

[0060] In this embodiment of the application, when the weighted sum of squared distances (wdss) from the target point cloud to the target tool contact plane calculated in step S104 is less than the specified threshold MinWDSS, or the aforementioned iteration round iterIndex reaches the maximum round MaxIter, indicating that the stopping condition is met, the tool pose vector of the path key points is constructed in conjunction with the current tool position, and after calculating the tool pose vector of all path key points, a smooth tool pose path is obtained. For example, as... Figure 2 As shown, if the stopping condition is not met, the next iteration continues and the iterIndex value is incremented by one.

[0061] Optionally, when constructing the tool pose vector of the path key points, the Euler angle data of the tool pose obtained by iterative calculation is determined; the Euler angle data and the position coordinates of the path key points are merged accordingly to generate the tool pose vector of the path key points.

[0062] Specifically, for example, the coordinate data (x, y, z) of the location coordinates kpi of the path key points are combined with the Euler angle data (rx, ry, rz) of the tool pose rotVec obtained by iterative calculation to form a six-dimensional tool pose vector vecPose, with pose data of (x, y, z, rx, ry, rz).

[0063] Optionally, when obtaining a smooth tool pose path based on the tool pose vectors of all path key points, a preset path pose interpolation method is used to perform dense interpolation on the path key points to generate original path points; the original path points are then filtered, and a smooth tool pose path is output based on the tool pose vectors corresponding to the filtered original path points. The path pose interpolation method can be, for example, linear pose interpolation.

[0064] Specifically, a suitable path pose interpolation algorithm can be used to perform dense interpolation of the path key points with the required step size to generate the original path points, such as straight line pose interpolation. Then, a filtering algorithm that meets the requirements is used to filter the original path points to smooth the path points, such as Gaussian filtering algorithm, and finally a smooth tool pose path is obtained. At this point, the tool pose path planning is completed.

[0065] In summary, this application uses the original computational data of point cloudification of the target surface and multi-plane circular approximation of the tool contact surface. Based on iterative pose and gradient step size adjustment of the tool pose, and using the weighted sum of squared distances as a metric for the fit between the tool and the target, it proposes a clear standard for measuring the fit of complex surfaces, filling the technical gap in tool fit planning with multiple contact surfaces.

[0066] The path planning method provided in this application embodiment acquires surface point cloud data of the target object in the task through a preset point cloud acquisition device, converts the surface point cloud data into general point cloud data, defines the contact surface of the tool to obtain the planar data of the target tool contact plane, and performs path interpolation on the task points based on the target tool contact plane to obtain path key points. Each path key point is sequentially pushed into the path key point queue to be processed. For each path key point in the path key point queue, the target point cloud is acquired, and the tool pose is adjusted based on the target point cloud under a preset iteration round. The planar data of the target tool contact plane is converted based on the tool pose. The weighted sum of squared distances from the target point cloud to the target tool contact plane is calculated based on the converted planar data. When the weighted sum of squared distances is less than a specified threshold or the iteration round reaches the maximum round, the tool pose vector of the path key points is constructed. After calculating the tool pose vectors of all path key points, a smooth tool pose path is obtained based on the tool pose vectors of all path key points. The path planning method of this application calculates the tool pose vector of the path key points by using the point cloud of the surface of the task target object and the contact plane data of the multi-plane circular approximation target tool defined by the user, and then generates the tool pose path by interpolating and smoothing all the path key points. This allows it to be applied in task environments with obvious undulations of multiple contact surfaces of the tool, thus improving the versatility of path planning.

[0067] Figure 3 This is a schematic diagram of the path planning device provided according to an embodiment of this application; as shown below. Figure 3 As shown, the path planning device 300 of this application embodiment may specifically include: The conversion module 301 is used to acquire surface point cloud data of the target object in the task through a preset point cloud acquisition device, and convert the surface point cloud data into general point cloud data.

[0068] The first acquisition module 302 is used to define the contact surface of the tool, obtain the planar data of the contact plane of the target tool and the contact plane of the target tool, and perform path interpolation on the task points based on the contact plane of the target tool to obtain the path key points.

[0069] The adjustment module 303 is used to push each path key point into the path key point queue to be processed in sequence, and for each path key point in the path key point queue, to obtain the target point cloud, and to adjust the tool pose based on the target point cloud in a preset iteration cycle.

[0070] The calculation module 304 is used to convert the planar data of the target tool contact plane based on the tool pose, and calculate the weighted sum of squared distances from the target point cloud to the target tool contact plane based on the converted planar data.

[0071] The second acquisition module 305 is used to construct the tool pose vector of the path key points when the weighted sum of squared distances is less than a specified threshold or the number of iterations reaches the maximum number of iterations, and to obtain a smooth tool pose path based on the tool pose vector of all path key points after calculating the tool pose vector of all path key points.

[0072] In one possible implementation, the first acquisition module is specifically used for: Determine the tool installation center point and define it as the origin of the tool's own coordinate system to obtain the tool's own coordinate system; In its own tool coordinate system, with the tool installation center point as the origin, construct the center coordinates of the circle, the plane normal vector pointing to the origin, and the radius of the circle in its own tool coordinate system to obtain a planar circle; Based on the contact surface of the planar circular covering tool in practical applications, the contact plane of the target tool and the planar data of the contact plane of the target tool are obtained.

[0073] In one possible implementation, the first acquisition module is specifically used for: Determine the starting and ending coordinates of the task, and calculate the distance between the starting and ending points based on the vector difference between the starting and ending coordinates; Under the preset coordinate difference interpolation method, the ratio of the distance from the current path key point to the starting point coordinate to the starting point distance is obtained based on the starting point spacing and the preset interpolation step size. Based on the scale, starting point coordinates, and ending point coordinates, the position coordinates of each key point on the path are calculated sequentially according to the scale.

[0074] In one possible implementation, the adjustment module is specifically used for: A spatial search tree is constructed based on general point cloud data. The target point cloud is obtained by using the target point radius search method of the spatial search tree to search for the location coordinates of the key points of the path and all points within the preset tool radius. The preset tool radius is determined according to the radius of the activity range corresponding to each contact surface of the tool. The tool pose is adjusted according to the current gradient descent direction with a preset number of iterations and a corresponding adjustment step size to obtain the adjusted tool pose; wherein, the adjustment step size gradually decreases as the number of iterations increases.

[0075] In one possible implementation, the computing module is specifically used for: The center coordinates and plane normal vector of the target tool contact plane are multiplied by the tool pose to obtain the transformed center coordinates and plane normal vector. Points within the circular radius of each target tool's contact plane are selected to form a planar circular neighbor point cloud. Based on the coordinates of each point in the planar circular neighbor point cloud, combined with the transformed center coordinates and the plane normal vector, the target distance from each point to the corresponding target tool's contact plane is calculated. Here, the circular radius represents the circular radius of each tool's contact plane. The sum of squared distances from each point in the planar circular neighbor point cloud to the contact plane of the target tool is calculated based on the target distance. The weighted sum of squared distances from the target point cloud to the contact plane of the target tool is then calculated based on the sum of squared distances and the planar circular weights of the corresponding target tool contact plane.

[0076] In one possible implementation, the second acquisition module is specifically used for: Determine the Euler angle data of the tool pose obtained from iterative calculation; The Euler angle data and the position coordinates of the path key points are merged to generate the tool pose vector of the path key points.

[0077] In one possible implementation, the second acquisition module is specifically used for: The path key points are densely interpolated using a preset path pose interpolation method to generate the original path points; The original path points are filtered, and a smooth tool pose path is output based on the tool pose vector corresponding to the filtered original path points.

[0078] The path planning device provided in this application embodiment acquires surface point cloud data of the target object in the task through a preset point cloud acquisition device, converts the surface point cloud data into general point cloud data, defines the contact surface of the tool to obtain the planar data of the target tool contact plane, and performs path interpolation on the task points based on the target tool contact plane to obtain path key points. Each path key point is sequentially pushed into the path key point queue to be processed. For each path key point in the path key point queue, the target point cloud is acquired, and the tool pose is adjusted based on the target point cloud under a preset iteration round. The planar data of the target tool contact plane is converted based on the tool pose. The weighted sum of squared distances from the target point cloud to the target tool contact plane is calculated based on the converted planar data. When the weighted sum of squared distances is less than a specified threshold or the iteration round reaches the maximum round, the tool pose vector of the path key points is constructed. After calculating the tool pose vectors of all path key points, a smooth tool pose path is obtained based on the tool pose vectors of all path key points. The path planning device of this application calculates the tool pose vector of the path key points by using the point cloud of the surface of the task target object and the contact plane data of the multi-plane circular approximation target tool defined by the user, and generates the tool pose path after interpolating and smoothing all the path key points. This allows it to be applied in task environments with obvious undulations of multiple contact surfaces of the tool, thus improving the versatility of path planning.

[0079] like Figure 4 As shown in the embodiment of this application, an electronic device 400 includes a processor 401, a memory 402, and a bus. The memory 402 stores machine-readable instructions that can be executed by the processor 401. When the electronic device is running, the processor 401 communicates with the memory 402 via the bus, and the processor 401 executes the machine-readable instructions to perform the steps of the path planning method described above.

[0080] Specifically, the memory 402 and processor 401 mentioned above can be general-purpose memory and processor, without any specific limitations. When the processor 401 runs the computer program stored in the memory 402, it can execute the above-mentioned path planning method.

[0081] Corresponding to the path planning method described above, this application also provides a computer-readable storage medium storing a computer program, which, when executed by a processor, performs the steps of the path planning method described above.

[0082] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working processes of the systems and devices described above can be referred to the corresponding processes in the method embodiments, and will not be repeated here. In the several embodiments provided in this application, it should be understood that the disclosed systems, devices, and methods can be implemented in other ways. The device embodiments described above are merely illustrative. For example, the division of modules is only a logical functional division, and in actual implementation, there may be other division methods. Furthermore, multiple modules or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the displayed or discussed mutual coupling or direct coupling or communication connection can be through some communication interfaces; the indirect coupling or communication connection of devices or modules can be electrical, mechanical, or other forms.

[0083] The modules described as separate components may or may not be physically separate. The components shown as modules may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0084] In addition, the functional units in the various embodiments of this application can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit.

[0085] If the aforementioned functions are implemented as software functional units and sold or used as independent products, they can be stored in a processor-executable, non-volatile, computer-readable storage medium. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or a portion of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the deployment methods described in the various embodiments of this application. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, ROM, RAM, magnetic disks, or optical disks.

[0086] The above are merely specific embodiments of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A path planning method, characterized in that, The method includes: The surface point cloud data of the target object in the task is acquired through a preset point cloud acquisition device, and the surface point cloud data is converted into general point cloud data. Define the contact surface of the tool to obtain the planar data of the contact plane of the target tool and the contact plane of the target tool, and perform path interpolation on the task points based on the contact plane of the target tool to obtain the path key points; Each path key point is pushed into the path key point queue to be processed in sequence. For each path key point in the path key point queue, the target point cloud is obtained, and the tool pose is adjusted based on the target point cloud in a preset iteration cycle. Based on the tool pose, the planar data of the target tool contact plane is transformed, and the weighted sum of squared distances from the target point cloud to the target tool contact plane is calculated based on the transformed planar data; When the weighted sum of squared distances is less than a specified threshold or the number of iterations reaches the maximum number of iterations, the tool pose vectors of the path key points are constructed, and after calculating the tool pose vectors of all path key points, a smooth tool pose path is obtained based on the tool pose vectors of all path key points.

2. The method according to claim 1, characterized in that, The definition of the contact surface of the tool, obtaining planar data of the contact plane of the target tool and the contact plane of the target tool, includes: Determine the tool installation center point and define the tool installation center point as the origin of its own tool coordinate system; In the self-tool coordinate system, with the tool installation center point as the origin, construct the center coordinates of the self-tool coordinate system, the plane normal vector pointing to the origin, and the radius of the circle to obtain a planar circle; Based on the contact surface of the planar circular covering tool in actual application, the contact plane of the target tool and the planar data of the contact plane of the target tool are obtained.

3. The method according to claim 1, characterized in that, The step of interpolating the path to the task points based on the contact plane of the target tool to obtain the path key points includes: Determine the starting point coordinates and ending point coordinates of the task, and calculate the distance between the starting point and ending point based on the vector difference between the starting point coordinates and the ending point coordinates; Under the preset coordinate difference interpolation method, the ratio of the distance from the current path key point to the starting point coordinate to the starting point distance is obtained based on the starting point distance and the preset interpolation step size. Based on the ratio, the starting point coordinates, and the ending point coordinates, the position coordinates of each key point on the path are calculated sequentially according to the ratio.

4. The method according to claim 1, characterized in that, The step of acquiring the target point cloud and adjusting the tool pose based on the target point cloud in a preset number of iterations includes: A spatial search tree is constructed based on the general point cloud data. The target point cloud is then searched using the target point radius search method of the spatial search tree to find all points within the location coordinates of the path key points and the preset tool radius. The preset tool radius is determined based on the radius of the activity range corresponding to each contact surface of the tool. The tool pose is adjusted according to the current gradient descent direction with a preset number of iterations and a corresponding adjustment step size to obtain the adjusted tool pose; wherein the adjustment step size gradually decreases as the number of iterations increases.

5. The method according to claim 2, characterized in that, The step of converting the planar data of the target tool contact plane based on the tool pose, and calculating the weighted sum of squared distances from the target point cloud to the target tool contact plane based on the converted planar data, includes: The center coordinates and plane normal vector of the target tool contact plane are multiplied by the tool pose to obtain the transformed center coordinates and plane normal vector. Points within the radius of a circle on each target tool's contact plane are selected to form a planar circular neighbor point cloud. Based on the coordinates of each point in the planar circular neighbor point cloud, combined with the transformed center coordinates and the plane normal vector, the target distance from each point to the corresponding target tool's contact plane is calculated. Here, the circular radius represents the circular radius of each tool contact plane of the tool. Based on the target distance, the sum of squared distances from each point in the planar circular neighbor point cloud to the target tool contact plane is calculated. Based on the sum of squared distances and the planar circular weights of the corresponding target tool contact plane, the weighted sum of squared distances from the target point cloud to the target tool contact plane is calculated.

6. The method according to claim 1, characterized in that, The tool pose vector for constructing the path key points includes: Determine the Euler angle data of the tool pose obtained from iterative calculation; The Euler angle data and the position coordinates of the path key points are merged to generate the tool pose vector of the path key points.

7. The method according to claim 1, characterized in that, The process of obtaining a smoothed tool pose path based on the tool pose vectors of all path key points includes: The path key points are densely interpolated using a preset path pose interpolation method to generate the original path points; The original path points are filtered, and a smooth tool pose path is output based on the tool pose vector corresponding to the filtered original path points.

8. A path planning device, characterized in that, The device includes: The conversion module is used to acquire surface point cloud data of the target object in the task through a preset point cloud acquisition device, and convert the surface point cloud data into general point cloud data. The first acquisition module is used to define the contact surface of the tool to obtain the planar data of the contact plane of the target tool and the contact plane of the target tool, and to perform path interpolation on the task points based on the contact plane of the target tool to obtain the path key points; The adjustment module is used to push each path key point into the path key point queue to be processed in sequence, and for each path key point in the path key point queue, obtain the target point cloud, and adjust the tool pose based on the target point cloud in a preset iteration cycle. The calculation module is used to convert the planar data of the target tool contact plane based on the tool pose, and calculate the weighted sum of squared distances from the target point cloud to the target tool contact plane based on the converted planar data; The second acquisition module is used to construct the tool pose vector of the path key points when the weighted sum of squared distances is less than a specified threshold or the iteration round reaches the maximum round, and to obtain a smooth tool pose path based on the tool pose vector of all path key points after calculating the tool pose vector of all path key points.

9. An electronic device, characterized in that, include: The device includes a processor, a memory, and a bus. The memory stores machine-readable instructions executable by the processor. When the electronic device is running, the processor communicates with the memory via the bus. When the machine-readable instructions are executed by the processor, they perform the steps of the path planning method as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program that, when executed by a processor, performs the steps of the path planning method as described in any one of claims 1 to 7.

Images