Flanging path generation method, device and equipment, cleaning robot and storage medium
Through the smoothing processing of the edge path of the cleaning robot and the optimization of the path control point, the problem of unsmooth edge path in the border area of the cleaning robot is solved, and the overall smoothness and cleaning efficiency of the path are improved.
Patent Information
- Application Number
- CN202310567334.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2023-05-18
- Publication Date
- 2024-11-26
- Estimated Expiration
- 2043-05-18
AI Technical Summary
When cleaning the boundary area, the edge path is not smooth due to the noise and error of the boundary contour point, which shows a wave shape and insufficient overall smoothness.
By obtaining the original edge path of the target area, performing smoothing processing and path control point optimization, building an optimization objective function to improve the overall smoothness and motion constraints of the path, and optimizing path control points using B-spline curve fitting and nonlinear optimization algorithms , make the two adjacent curved edge paths bend to the same side.
The overall optimization of the edge-mounted path is achieved, the local path radian is reduced, the smoothness and rationality of the path are improved, and the cleaning robot can clean the boundary area more efficiently.
Smart Images

Figure CN119024684A_ABST
Abstract
Description
Technical Field
[0001] The present application relates to the field of path planning, and in particular to a method, device, equipment, cleaning robot and storage medium for generating an edge-adjacent path. Background Art
[0002] Cleaning robots are robots that replace humans in cleaning, which have emerged in recent years with the advancement of science and technology. Cleaning robots include a body, processor, motor, drive wheel, positioning sensor, and various cleaning components, which are used to realize automatic walking and positioning of the cleaning robot, as well as the treatment of dirty areas.
[0003] When performing cleaning tasks, the cleaning robot will perform additional cleaning on the border area according to the separately planned edge path after cleaning the internal area, so as to ensure the cleanliness of the edge cleaning. Due to the noise and error of the boundary contour points of the cleaning area, and the lack of consistency, the planned edge path will appear wavy, swaying back and forth, and the overall smoothness is not enough. Summary of the invention
[0004] The present application provides a method, device, equipment, cleaning robot and storage medium for generating an edge path to solve the technical problem that the overall smoothness of the edge path is insufficient due to the lack of consistency of the boundary contour points of the cleaning area.
[0005] In a first aspect, a method for generating an edge path is provided, comprising:
[0006] Acquire an original edge path corresponding to a target area, wherein the target area is an area of a current edge path to be planned, and the original edge path includes a plurality of ordered boundary points of the target area;
[0007] Based on the multiple ordered boundary points, the original edge path is smoothed to obtain a smooth edge path corresponding to the target area, wherein the smooth edge path includes multiple curved edge paths connected end to end;
[0008] The path control points of the multi-segment curved edge path are optimized so that two adjacent curved edge paths in the multi-segment curved edge path bend to the same side, thereby obtaining a planned edge path of the target area. The path control points are used to control the bending direction and shape of the curved edge path.
[0009] In this technical solution, an original edging path corresponding to a target area is obtained, the original edging path includes a plurality of ordered boundary points of the target area, and then based on the plurality of ordered boundary points on the original edging path, the original edging path is smoothed to obtain a smooth edging path corresponding to the target area, the smooth edging path includes a plurality of curved edging paths connected end to end, thereby realizing local optimization of the edging path, so that the local path in the edging path is a smooth curve; then, the path control points of the multi-segment curved edging paths on the smooth edging path are optimized, so that two adjacent curved edging paths in the multi-segment curved edging paths are bent to the same side, so as to obtain a planned edging path for the target area, thereby realizing overall optimization of the edging path, and by optimizing the path control points of the multi-segment curved edging paths on the smooth edging path, so that two adjacent curved edging paths in the smooth edging path are bent to the same side, the curvature of the two adjacent local paths in the edging path can be reduced, presenting a local path straightening effect, thereby making the overall curvature of the edging path smaller and smoother, and thus the overall smoothness of the edging path is higher.
[0010] In combination with the first aspect, in a possible implementation, the path control points of the multi-segment curve edge path are optimized so that two adjacent segments of the multi-segment curve edge path bend to the same side, and the planned edge path of the target area is obtained, including: constructing an optimization objective function related to the path control points, the optimization objective function uses the path control points as independent variables, and the optimization objective function is used to improve the overall smoothness of the path; using the path control points of the multi-segment curve edge path as the initial solution of the independent variables in the optimization objective function, and determining the optimal solution of the independent variables to obtain a new path control point, the optimal solution minimizes the function value of the optimization objective function; and generating the planned edge path according to the new path control point. By modeling path smoothness as an optimization problem for path control points, constructing an optimization objective function, and solving the optimal solution of the optimization objective function, accurate optimization of the edge path can be achieved.
[0011] In conjunction with the first aspect, in a possible implementation, the optimization objective function is a nonlinear function related to the path control points. The optimization objective function is a nonlinear function related to the path control points, which is equivalent to optimizing the path in a nonlinear manner, which is more in line with the characteristics of path planning, thereby helping to improve the rationality of edge path optimization.
[0012] In combination with the first aspect, in a possible implementation, the optimization objective function includes a smoothness optimization objective function and a motion constraint objective function, the function value of the optimization objective function is obtained based on the function value of the smoothness optimization objective function and the function value of the motion constraint objective function, the smoothness optimization objective function is used to optimize the overall smoothness of the path, and the motion constraint function is used to constrain the speed and acceleration on the path. By setting the optimization objectives of path smoothness and kinematics, the path finally optimized can be made more reasonable.
[0013] In combination with the first aspect, in a possible implementation manner, the smoothness optimization objective function is:
[0014]
[0015] Among them, f s The function value of the objective function for smoothness optimization, Q i+1 , Q i and Q i-1 is the position vector of three adjacent path control points, and pb is the order of the curve corresponding to the curve edge path.
[0016] In combination with the first aspect, in a possible implementation manner, the motion constraint objective function includes:
[0017]
[0018]
[0019] Among them, f v (v t ) is the function value of the speed curve function, f a (v a ) is the function value of the acceleration curve function, v max and a max are the preset maximum speed and maximum acceleration, v t and a t are the velocity and acceleration on the curve edge path, v t and a t The path control points of the curve-fitting path are obtained.
[0020] In combination with the first aspect, in a possible implementation, the function value of the optimization objective function is obtained by weighted summing the function value of the smoothness optimization objective function and the function value of the motion constraint objective function. By setting weights for the optimization objectives of path smoothness and kinematics, the final optimized path can be made more reasonable.
[0021] In combination with the first aspect, in a possible implementation, the smoothing of the original edge path based on the multiple ordered boundary points to obtain the smoothed edge path corresponding to the target area includes: performing curve fitting on the multiple ordered boundary points to obtain the smoothed edge path. By means of curve fitting, local optimization of the edge path can be achieved.
[0022] In combination with the first aspect, in a possible implementation, performing curve fitting on the multiple ordered boundary points to obtain the smooth edge path includes: performing B-spline curve fitting on the multiple ordered boundary points to obtain the smooth edge path. Performing curve fitting on the multiple ordered boundary points based on the B-spline curve can make the fitted edge path smoother and more similar to the original edge path.
[0023] In combination with the first aspect, in a possible implementation, the obtaining of the original edge-fitting path corresponding to the target area includes: obtaining target three-dimensional point cloud data corresponding to the target area, the target three-dimensional point cloud data being used to characterize the target area; extracting three-dimensional point cloud data representing the contour of the target area from the target three-dimensional point cloud data to obtain a plurality of boundary points corresponding to the target area; and sorting the plurality of boundary points to obtain the original edge-fitting path corresponding to the target area. By obtaining the three-dimensional point cloud data of the target area and extracting the three-dimensional point cloud data representing the contour as the boundary points of the target area, and then sorting the boundary points, the initial path of the target area can be obtained.
[0024] In combination with the first aspect, in a possible implementation, the extracting the three-dimensional point cloud data representing the outline of the target area from the target three-dimensional point cloud data to obtain a plurality of boundary points includes: extracting the three-dimensional point cloud data representing the outline of the target area from the target three-dimensional point cloud data based on a rolling ball method to obtain a plurality of boundary points corresponding to the target area. Extracting the three-dimensional point cloud data representing the outline of the target area based on the rolling ball method can achieve rapid extraction of boundary points of the target area.
[0025] In combination with the first aspect, in a possible implementation method, obtaining the target three-dimensional point cloud data corresponding to the target area includes: obtaining three-dimensional point cloud data containing the target area; performing target segmentation on the three-dimensional point cloud data to obtain the target three-dimensional point cloud data.
[0026] In combination with the first aspect, in a possible implementation method, obtaining three-dimensional point cloud data including the target area includes: obtaining a depth image including the target area; performing coordinate transformation on each pixel point in the depth image to obtain the three-dimensional point cloud data including the target area.
[0027] In a second aspect, a device for generating an edge path is provided, comprising:
[0028] A path acquisition module, used to acquire an original edge-adjacent path corresponding to a target area, wherein the target area is an area of the current edge-adjacent path to be planned, and the original edge-adjacent path includes a plurality of ordered boundary points of the target area;
[0029] A path smoothing module, configured to smooth the original edge path based on the plurality of ordered boundary points to obtain a smooth edge path corresponding to the target area, wherein the smooth edge path includes a plurality of curved edge paths connected end to end;
[0030] A path optimization module is used to optimize the path control points of the multi-segment curved edge path so that two adjacent curved edge paths in the multi-segment curved edge path bend to the same side to obtain the planned edge path of the target area. The path control points are used to control the bending direction and shape of the curved edge path.
[0031] In a third aspect, a computer device is provided, comprising a memory and one or more processors, wherein the memory is connected to the one or more processors, and the one or more processors are used to execute one or more computer programs stored in the memory, and when the one or more processors execute the one or more computer programs, the computer device implements the above-mentioned first aspect of the method for generating an edge path.
[0032] In a fourth aspect, a computer-readable storage medium is provided, wherein the computer-readable storage medium stores a computer program, wherein the computer program includes program instructions, and when the program instructions are executed by a processor, the processor executes the method for generating an edge path according to the first aspect.
[0033] In a fifth aspect, a cleaning robot is provided, comprising a mobile body and a cleaning robot arm, wherein the mobile cleaning robot is used to execute the edge path generation method of the first aspect, and the planned edge path is the edge path of the cleaning robot arm when cleaning the target area.
[0034] The present application can achieve the following technical effects: it achieves overall optimization of the edging path by optimizing the path control points of multiple curved edging paths on the smooth edging path so that two adjacent curved edging paths of the smooth edging path are bent to the same side, which can reduce the curvature of two adjacent local paths in the edging path, presenting a local path straightening effect, thereby making the overall curvature of the edging path smaller and smoother, and thus the overall smoothness of the edging path is higher. BRIEF DESCRIPTION OF THE DRAWINGS
[0035] Figure 1 A schematic diagram of a specific application scenario provided in an embodiment of the present application;
[0036] Figure 2 A schematic diagram of a flow chart of a method for generating an edge path provided in an embodiment of the present application;
[0037] Figure 3 A schematic diagram of an original welt path provided in an embodiment of the present application;
[0038] Figure 4 A schematic diagram of the relationship between the curve and the control point provided in the embodiment of the present application;
[0039] Figure 5 A schematic diagram of a smooth edge-adhesive path and a planned edge-adhesive path provided in an embodiment of the present application;
[0040] Figure 6 It is a structural schematic diagram of a device for generating an edge path provided in an embodiment of the present application;
[0041] Figure 7 It is a structural diagram of a computer device provided in an embodiment of the present application. DETAILED DESCRIPTION
[0042] The technical solutions in the embodiments of the present application will be described below in conjunction with the drawings in the embodiments of the present application.
[0043] The technical solution of the present application can be applied to path planning scenarios. The technical solution of the present application can be specifically applied to a target device. Optionally, the technical solution of the present application can also be applied to other devices that have a connection relationship or a supporting relationship with the target device, and the connection relationship between the target device and the other device can be a wired connection relationship or a wireless connection relationship. For example, the target device is a cleaning robot, and the other device can refer to a base station supporting the cleaning robot, or the other device can also refer to a background server (such as a cloud server) corresponding to the cleaning robot, and the background server and the cleaning robot transmit data based on wireless communication.
[0044] In a specific application scenario, the technical solution of the present application can be applied to a cleaning robot with a cleaning robot arm, wherein the cleaning robot with a cleaning robot arm uses the cleaning robot arm to clean the ground, walls and other spatial locations. The planned edge path generated based on the technical solution of the present application can be the edge path of the cleaning robot's cleaning robot arm to clean the target area. The cleaning robot's cleaning robot arm can move according to the planned edge path to complete the cleaning of the boundary of the target area. For example, reference can be made to Figure 1 In the scenario shown, when the cleaning robot 10 performs the cleaning task of the washbasin 20, it can generate a planned edge path along the edge of the washbasin 20 based on the technical solution of the present application, so that the edge of the washbasin 20 can be cleaned based on the planned edge path.
[0045] The technical solution of this application is described in detail below.
[0046] See also Figure 2 , Figure 2 A schematic diagram of a flow chart of a method for generating an edge path provided in an embodiment of the present application, which method can be applied to the target device mentioned above or other devices, such as Figure 2 As shown, the method comprises the following steps:
[0047] S301, obtaining an original edge path corresponding to the target area.
[0048] Here, the target area is the area where the edge path is currently being planned, and the target area can be, for example, the area to be cleaned. Figure 1 In the middle region R1, the dotted lines l1, l2, and l3 of the region R1 are all boundaries in the target region R1.
[0049] The original edge path corresponding to the target area refers to the edge path obtained by identifying the boundary of the target area. The original edge path includes multiple ordered boundary points of the target area. The ordered boundary points refer to ordered boundary points. The multiple ordered boundary points of the target area are connected in sequence according to their respective orders to obtain the original edge path corresponding to the target area. For example, reference can be made to Figure 3 , assuming the target area is Figure 1 The original edge path corresponding to the target area can be as follows: Figure 3 Indicated by the thick white solid line in the figure.
[0050] The original edge path corresponding to the target area can be obtained by identifying and sorting the boundary points of the target area. In a feasible implementation, the original edge path corresponding to the target area can be obtained by the following steps A1-A3:
[0051] A1. Obtain target 3D point cloud data corresponding to the target area.
[0052] The target 3D point cloud data is used to represent the target area, and the target 3D point cloud data is a collection of position points in the target area. Each position point in the target area can be represented in the form of (x, y, z) in the target 3D point cloud data, which is used to represent the position coordinates of the position point in the target area in space.
[0053] In a feasible implementation, the target three-dimensional point cloud data corresponding to the target area can be obtained through the following steps A11-A12:
[0054] A11. Obtain three-dimensional point cloud data containing the target area.
[0055] Here, the three-dimensional point cloud data is used to characterize the spatial area including the target area (hereinafter referred to as the target spatial area). The various spatial position points in the target spatial area constitute the three-dimensional point cloud data including the target area. The representation form of each three-dimensional point cloud data is the same. The specific representation form can refer to the target three-dimensional point cloud data.
[0056] In a specific implementation, three-dimensional point cloud data containing a target area can be obtained through passive acquisition. Specifically, a depth image containing the target area can be obtained, and each pixel in the depth image can be coordinate transformed to obtain three-dimensional point cloud data containing the target area. Among them, the depth image containing the target area can be obtained by photographing the target space area with a depth camera. Coordinate transformation is performed on each pixel in the depth image to obtain three-dimensional point cloud data containing the target area, which means that the pixel coordinates of each pixel in the depth image and the depth data of each pixel are converted to a camera coordinate system, a device coordinate system, or a world coordinate system through coordinate transformation. The specific coordinate system to which the pixels in the depth image are converted can be determined based on actual needs, and this application does not impose any restrictions.
[0057] For example, the target device is Figure 1 The cleaning robot 10 shown has a target area of Figure 1 In the area R2 shown in the figure, a depth camera is installed at the end of the cleaning arm of the cleaning robot. The depth camera can be used to shoot towards the area R2 to obtain a depth image, and then the coordinates of each pixel in the depth image are transformed to obtain the three-dimensional point cloud data containing the target area in the camera coordinate system. Furthermore, the three-dimensional point cloud data containing the target area can also be transformed from the camera coordinate system to the coordinate system of the mechanical arm. Assume that the coordinates of the three-dimensional point cloud data in the camera coordinate system are p1 i , then the coordinates of the three-dimensional point cloud data in the coordinate system of the robot are p2 i =T1*T2*p1 i , where T1 is the first transformation matrix, which is used to indicate the relative position relationship between the end of the robot arm and the base of the robot arm, including the rotation and translation relationship between the end of the robot arm and the base of the robot arm, and can be obtained by real-time detection of the position of the robot arm and based on kinematic calculation; T2 is the second transformation matrix, which is used to indicate the relative position relationship between the camera and the end of the cleaning robot arm, including the rotation and translation relationship between the camera and the end of the robot arm, and can be obtained by pre-calibration (such as hand-eye calibration method).
[0058] By acquiring a depth image containing the target area and performing coordinate transformation on each pixel in the depth image, three-dimensional point cloud data containing the target area is obtained, and the implementation method is simple.
[0059] Optionally, three-dimensional point cloud data containing the target area can also be obtained by active acquisition, such as by optical acquisition. Among them, the target space area can be scanned by a laser radar to obtain three-dimensional point cloud data containing the target area in the laser radar coordinate system. Similarly, the three-dimensional point cloud data in the laser radar coordinate system can also be converted to obtain three-dimensional point cloud data in the device coordinate system or in the world coordinate system. Alternatively, the three-dimensional point cloud data containing the target area can also be obtained by means of sound waves or electromagnetic waves.
[0060] Optionally, the three-dimensional point cloud data including the target area can also be obtained by combining active and passive acquisition. The present application does not limit the method for obtaining the three-dimensional point cloud data including the target area.
[0061] A12. Perform target segmentation on the three-dimensional point cloud data containing the target area to obtain target three-dimensional point cloud data.
[0062] Here, performing target segmentation on the three-dimensional point cloud data including the target area refers to identifying the target area from the three-dimensional point cloud data including the target area and segmenting the three-dimensional point cloud data specifically belonging to the target area to obtain target three-dimensional point cloud data.
[0063] Specifically, any one of the region-based segmentation methods, such as region growing method, region separation aggregation, watershed, etc., can be used to perform target segmentation on the three-dimensional point cloud data containing the target area to obtain the target three-dimensional point cloud data.
[0064] In an optional implementation, after obtaining the three-dimensional point cloud data containing the target area, the three-dimensional point cloud data belonging to the edge of the target area, that is, the boundary points of the target area, can also be identified by an edge-based method, and then the area formed by connecting the boundary points is determined as the target area, and the three-dimensional point cloud data in the target area is determined as the target three-dimensional point cloud data. This application does not limit the specific method of obtaining the target three-dimensional point cloud data.
[0065] A2. Extracting the three-dimensional point cloud data representing the contour of the target area from the target three-dimensional point cloud data, and obtaining a plurality of boundary points corresponding to the target area.
[0066] In a feasible implementation, based on the rolling ball method, three-dimensional point cloud data representing the outline of the target area can be extracted from the target three-dimensional point cloud data to obtain a plurality of boundary points corresponding to the target area.
[0067] Specifically, the target three-dimensional point cloud data can be projected onto a two-dimensional plane (generally an XOY plane) to obtain the target two-dimensional point cloud data. Then, the contour points in the target two-dimensional point cloud data are determined through the following steps (11)-(14).
[0068] (11) For the point p (x, y) to be judged, according to the preset rolling circle radius α, search for all points whose distance from point p is less than 2α to obtain the point set P.
[0069] The point p to be determined is any point in the target two-dimensional point cloud.
[0070] (12) Select any point p1(x1, y1) in the point set P, and calculate the coordinates of the two circle centers o1(x o1 ,y o1 ) and o2(x o2 ,y o2 ), the center coordinates o1(x o1 ,y o1 ) and o2(x o2 ,y o2 ) is calculated as follows:
[0071] x o1 =x+(x1-x)*1 / 2-(y1-y)*H
[0072] y o1 =y+(y1-y)*1 / 2-(x-x1)*H
[0073] x o2 =x+(x1-x)*1 / 2+(y1-y)*H
[0074] y o2 =y+(y1-y)*1 / 2+(x-x1)*H
[0075]
[0076] S 2 =(x1-x) 2 +(y-y1) 2
[0077] (13) For all points in the point set except point p1, calculate the distances from all other points to the center o1 and o2. If the distances from all other points except point p1 to the center o1 or the center o2 are greater than α, then p is determined to be a contour point.
[0078] (14) If the distances from points other than point p1 to the center o1 and the center o2 are not all greater than α, then select other points in the point set P as point p1 and return to step (12) until p is determined to be a contour point or all points in the point set P are traversed.
[0079] (15) If p is not determined to be a contour point after traversing all points in the point set P, then it is determined that p is not a contour point.
[0080] By judging each point in the target two-dimensional point cloud data through the above steps (11)-(15), multiple contour points in the target two-dimensional point cloud can be determined, and finally the three-dimensional point cloud data corresponding to the multiple contour points are determined as multiple boundary points corresponding to the target area.
[0081] The three-dimensional point cloud data representing the contour of the target area is extracted based on the rolling ball method, which can realize the rapid extraction of the boundary points of the target area.
[0082] Optionally, based on other boundary extraction methods, three-dimensional point cloud data representing the contour of the target area can be extracted from the target three-dimensional point cloud data to obtain multiple boundary points corresponding to the target area. For example, three-dimensional point cloud data representing the contour of the target area can also be extracted by edge extraction operators (such as Laplace operators and Sobel operators). Alternatively, three-dimensional point cloud data representing the contour of the target area can also be extracted from the target three-dimensional point cloud data based on the Delaunay contour extraction method to obtain multiple boundary points corresponding to the target area. This application is not limited.
[0083] A3. Sort multiple boundary points corresponding to the target area to obtain an original edge path corresponding to the target area.
[0084] Specifically, the multiple boundary points corresponding to the target area can be sorted by any of the scattered point sorting methods such as the nearest search method, the distance sorting method and the arrangements method to obtain the original edge path corresponding to the target area.
[0085] By acquiring the three-dimensional point cloud data of the target area and extracting the three-dimensional point cloud data representing the contour as the boundary points of the target area, and then sorting the boundary points, the initial path of the target area can be obtained.
[0086] Optionally, the original edge path corresponding to the target area may also be obtained through other implementations, which is not limited in this application.
[0087] S302, based on the multiple ordered boundary points of the target area, smoothing the original edge path corresponding to the target area to obtain a smoothed edge path corresponding to the target area.
[0088] Here, the smoothed edge path corresponding to the target area includes a plurality of curved edge paths connected end to end. Smoothing the original edge path corresponding to the target area means that for a turning position (hereinafter referred to as the turning position) in the original edge path corresponding to the target area, the curvature of the path at the turning position is reduced so that the path at the turning position becomes smooth. The shape of the smoothed edge path corresponding to the target area is similar to the shape of the original edge path corresponding to the target area.
[0089] In some possible cases, the original edge-fitting path corresponding to the target area may be smoothed by a path smoothing method based on interpolation or fitting to obtain a smoothed edge-fitting path corresponding to the target area.
[0090] In a feasible implementation, curve fitting may be performed on a plurality of ordered boundary points of the target area to obtain a smooth edge path corresponding to the target area.
[0091] Specifically, a B-spline curve can be fitted to multiple ordered boundary points of the target area to obtain a smooth edge path corresponding to the target area. The parameterized expression of the B-spline is:
[0092]
[0093] Among them, N i,p (t) is the basis function, Q i is the path control point, and the number of path control points is n.
[0094] First, multiple ordered boundary points of the target area can be parameterized to obtain B-spline parameter values, and then the node vectors can be determined according to the B-spline parameter values. Then, the basis functions and path control points can be solved according to the B-spline parameter values and the node vectors. Finally, the smooth edge path corresponding to the target area can be drawn according to the path control points and basis functions.
[0095] Curve fitting is performed on multiple ordered boundary points based on the B-spline curve, so that the fitted edge path is smoother and has a higher similarity to the original edge path.
[0096] Optionally, Bezier curve fitting can be performed on multiple ordered boundary points of the target area to obtain a smooth edge path corresponding to the target area. Alternatively, interpolation fitting can be performed on multiple ordered boundary points of the target area based on a polynomial to obtain a smooth edge path corresponding to the target area.
[0097] Optionally, multiple ordered boundary points may be divided into multiple groups of ordered boundary points, and each group of ordered boundary points may be interpolated or fitted to obtain multiple curved edge paths; and then the multiple curved edge paths may be connected to obtain a smooth edge path corresponding to the target area.
[0098] In some other possible situations, the original edge path corresponding to the target area can be smoothed based on the optimized path smoothing method to obtain a smoothed edge path corresponding to the target area. Alternatively, the original edge path corresponding to the target area can be smoothed based on the Floyd algorithm to obtain a smoothed edge path corresponding to the target area. This application does not limit the specific implementation method of the smoothing process.
[0099] S303, optimizing the path control points of the multiple curved edge paths connected end to end on the smooth edge path, so that two adjacent curved edge paths in the multiple curved edge paths bend to the same side, and obtaining a planned edge path of the target area.
[0100] Here, the path control point is used to control the bending direction and shape of the curve edge path. A curve edge path can correspond to multiple control points. The number of control points corresponding to a curve edge path is related to the order of the curve corresponding to the curve edge path. For example, you can refer to Figure 4 , Figure 4 The curve SE in is a third-order B-spline curve, so the number of control points corresponding to the curve SE is 4, which are Figure 4 Q1, Q2, Q3 and Q4 in . Different curve edge paths can correspond to the same control point. For example, if the curves corresponding to the multiple curve edge paths connected end to end on the smooth edge path are all third-order B-spline curves, then each curve edge path corresponds to 4 control points, curve edge path 1 corresponds to control points Q1, Q2, Q3 and Q4, curve edge path 2 corresponds to Q2, Q3, Q4 and Q5, curve edge path 3 corresponds to Q3, Q4, Q5 and Q6, ..., curve edge path n-3 corresponds to Q n-3 , Q n-1 , Q n-1 and Q n , n is the number of path control points. The number of path control points depends on the number of curve edge paths on the smooth edge path and the order of the curve corresponding to the curve edge path.
[0101] In some possible cases, the path control points may be optimized by constructing an optimization objective function for the path control points and determining the optimal solution of the optimization function, thereby optimizing the edge path of the target area. The above step S303 includes the following steps B1-B3.
[0102] A1. Construct an optimization objective function related to the path control points.
[0103] Here, the optimization objective function is a function with the path control point as an independent variable, and the optimization objective function can be expressed as f(Q).
[0104] The optimization objective function is used to improve the overall smoothness of the path. When the function value of the optimization objective function reaches the minimum, the overall smoothness of the path is the highest. The optimization objective function can be any objective function that can improve the overall smoothness of the path.
[0105] Specifically, the optimization objective function may be a nonlinear function related to the path control points. The optimization objective function is a nonlinear function related to the path control points, which is equivalent to optimizing the path in a nonlinear manner, which is more in line with the characteristics of path planning, thereby helping to improve the rationality of edge path optimization.
[0106] In a feasible implementation, the optimization objective function may include a smoothness optimization objective function and a motion constraint objective function, wherein the smoothness optimization objective function is used to optimize the overall smoothness of the path, the motion constraint function is used to constrain the speed and acceleration on the path, and the function value of the optimization objective function is obtained based on the function value of the smoothness optimization objective function and the function value of the motion constraint objective function. By setting the optimization objectives of the path smoothness and kinematics, the final optimized path can be made more reasonable.
[0107] Specifically, the smoothness optimization objective function can be:
[0108]
[0109] Among them, f s The function value of the objective function for smoothness optimization, Q i+1 , Q i and Q i-1 is the position vector of the three adjacent path control points, and pb is the order of the curve corresponding to the curve edge path. For example, the target area smooth edge path is obtained by B-spline curve fitting, and the B-spline curve used in the fitting is a 3rd order B-spline curve, then pb is 3.
[0110] Specifically, the motion constraint objective function may include:
[0111]
[0112]
[0113] Among them, f v (v t ) is the function value of the speed curve function, f a (v a ) is the function value of the acceleration curve function, v max and a max are the preset maximum speed and maximum acceleration, respectively. The maximum speed and maximum acceleration here are the maximum speed and maximum acceleration in Cartesian space, v t and a t are the velocity and acceleration on the curve edge path, v t and a t The path control points are obtained based on the curve edge path. For example, the target area smooth path is obtained by B-spline curve fitting.
[0114] Therefore, the optimization objective function can be: f1(Q)=fs+f v (vt )+f a (v a ), f1(Q) is the function value of the optimization objective function.
[0115] It should be noted that the smoothness optimization objective function and the motion constraint objective function are not limited to the above forms and can be set based on actual conditions.
[0116] In some possible cases, the function value of the optimization objective function can be obtained by weighted summing the function value of the smoothness optimization objective function and the function value of the motion constraint objective function. The optimization objective function can be: f2(Q)=w1*fs+w2*[f v (v t )+f a (v a )], f2(Q) is the function value of the optimization objective function, w1 and w2 are the weights of the smoothness optimization objective function and the motion constraint objective function respectively.
[0117] The weight of the motion constraint objective function may be greater than the weight of the smoothness optimization objective function. In a specific example, w2=10, w1=1.
[0118] By setting weights for the optimization objectives of path smoothness and kinematics, the final optimized path can be made more reasonable.
[0119] A2. The path control points of the multi-segment curve edge path are used as the initial solution of the independent variable in the optimization objective function, and the optimal solution of the independent variable is determined to obtain a new path control point.
[0120] Among them, the optimal solution of the independent variable minimizes the function value of the optimization objective function.
[0121] Specifically, the optimal solution of the independent variable in the optimization objective function can be solved by Newton's method or L-BFGS (large bfgs) algorithm.
[0122] The overall principle of solving the optimal solution of the independent variable in the optimization objective function by Newton's method can be as follows:
[0123] (21) Deriving the optimization objective function f(Q), we obtain the derivative functions f'(Q) and f"(Q) of the optimization objective function.
[0124] (22) Set k to 0.
[0125] (23) Solve the target of the independent variable Q k Substitute the optimization objective function f(Q) and the derivative function f'(Q) and f"(Q) to obtain the function value f of the optimization objective function f(Q), the derivative function f'(Q) and f"(Q)k 、f' k and f" k When k is 0, the target solution Q of the independent variable is k is the initial solution for the independent variable.
[0126] (24) According to the function value f 1k and f 2k and the target solution Q of the independent variable k , update the target solution Q of the independent variable k , get the next target solution Q of the independent variable k+1 ;
[0127] (25) Solve the target of the independent variable Q k+1 Substitute the optimization objective function f(Q) and the derivative function f'(Q) to obtain the function value f of the optimization objective function f(Q) and the derivative function f'(Q) k+1 、f' k+1 ;
[0128] (26) Determine f k+1 Is it less than f? k , judge f' k+1 Is it less than the preset threshold?
[0129] (27) If f k+1 Less than f k , and f' k+1 If the value is less than the preset threshold, the target Q k+1 Determine the optimal solution for the independent variable; otherwise, add 1 to k and return to step (23).
[0130] A3. Generate a planned edge path for the target area based on the new path control points.
[0131] Specifically, the planned edge-fitting path of the target area can be drawn according to the new path control points and in combination with the basis functions of the curve corresponding to the curve edge-fitting path.
[0132] For example, reference may be made to Figure 5 , Figure 5 The black dots in the figure are ordered boundary points, and the black dots are the original path control points. The smooth edge path corresponding to the original path control points is as follows: Figure 5 As shown by the dotted line in Figure 5 The gray triangle in the figure is the new path control point. The planned edge path corresponding to the new path control point is as follows: Figure 5 As shown in the black solid line.
[0133] By modeling path smoothing as an optimization problem for path control points, constructing an optimization objective function, and solving the optimal solution of the optimization objective function, accurate optimization of the edge-contacting path can be achieved.
[0134] In the above Figure 2 In the corresponding technical solution, an original edging path corresponding to the target area is obtained, the original edging path includes multiple ordered boundary points of the target area, and then based on the multiple ordered boundary points on the original edging path, the original edging path is smoothed to obtain a smooth edging path corresponding to the target area, the smooth edging path includes multiple curved edging paths connected end to end, thereby realizing local optimization of the edging path, so that the local path in the edging path is a smooth curve; then the path control points of the multiple curved edging paths on the smooth edging path are optimized, so that the two adjacent curved edging paths in the multiple curved edging paths are bent to the same side, so as to obtain the planned edging path of the target area, thereby realizing overall optimization of the edging path, and by optimizing the path control points of the multiple curved edging paths on the smooth edging path, the two adjacent curved edging paths in the smooth edging path are bent to the same side, so that the curvature of the two adjacent local paths in the edging path can be reduced, presenting a local path straightening effect, thereby making the overall curvature of the edging path smaller and smoother, and thus the overall smoothness of the edging path is higher.
[0135] The method of the present application is introduced above, and the device of the present application is introduced below.
[0136] See also Figure 6 , Figure 6 1 is a schematic diagram of the structure of a device for generating a path along the edge provided by an embodiment of the present application. The device for generating a path along the edge may be the target device or other device mentioned above. Figure 6 As shown, the edge path generating device 40 includes:
[0137] A path acquisition module 401 is used to acquire an original edge-adjacent path corresponding to a target area, where the target area is an area where an edge-adjacent path is currently to be planned, and the original edge-adjacent path includes a plurality of ordered boundary points of the target area;
[0138] A path smoothing module 402 is used to smooth the original edge path based on the multiple ordered boundary points to obtain a smooth edge path corresponding to the target area, wherein the smooth edge path includes multiple curved edge paths connected end to end;
[0139] The path optimization module 403 is used to optimize the path control points of the multi-segment curved edge path so that two adjacent curved edge paths in the multi-segment curved edge path bend to the same side, thereby obtaining the planned edge path of the target area. The path control points are used to control the bending direction and shape of the curved edge path.
[0140] In a possible design, the path optimization module 403 is specifically used to: construct an optimization objective function related to the path control points, the optimization objective function uses the path control points as independent variables, and the optimization objective function is used to improve the overall smoothness of the path; use the path control points of the multi-segment curve edge path as the initial solution of the independent variables in the optimization objective function, and determine the optimal solution of the independent variables to obtain a new path control point, and the optimal solution minimizes the function value of the optimization objective function; generate the planned edge path based on the new path control points.
[0141] In a possible design, the optimization objective function is a nonlinear function related to the path control points.
[0142] In one possible design, the optimization objective function includes a smoothness optimization objective function and a motion constraint objective function, and the function value of the optimization objective function is obtained based on the function value of the smoothness optimization objective function and the function value of the motion constraint objective function. The smoothness optimization objective function is used to optimize the overall smoothness of the path, and the motion constraint function is used to constrain the speed and acceleration on the path.
[0143] In a possible design, the smoothness optimization objective function is:
[0144]
[0145] Among them, f s The function value of the objective function for smoothness optimization, Q i+1 , Q i and Q i-1 is the position vector of three adjacent path control points, and pb is the order of the curve corresponding to the curve edge path.
[0146] In one possible design, the motion constraint objective function includes:
[0147]
[0148]
[0149] Among them, f v (v t ) is the function value of the speed curve function, f a (v a ) is the function value of the acceleration curve function, v max and a max are the preset maximum speed and maximum acceleration, v t and a t are the velocity and acceleration on the curve edge path, v t and a tThe path control points of the curve-fitting path are obtained.
[0150] In a possible design, the function value of the optimization objective function is obtained by weighted summing the function value of the smoothness optimization objective function and the function value of the motion constraint objective function.
[0151] In a possible design, the path smoothing module 402 is specifically used to perform curve fitting on the multiple ordered boundary points to obtain the smooth edge-fitting path.
[0152] In a possible design, the path smoothing module 402 is specifically used to perform B-spline curve fitting on the multiple ordered boundary points to obtain the smooth edge-fitting path.
[0153] In a possible design, the path acquisition module 401 is specifically used to: obtain target three-dimensional point cloud data corresponding to the target area, and the target three-dimensional point cloud data is used to characterize the target area; extract three-dimensional point cloud data representing the outline of the target area from the target three-dimensional point cloud data to obtain multiple boundary points corresponding to the target area; sort the multiple boundary points to obtain the original edge path corresponding to the target area.
[0154] In a possible design, the path acquisition module 401 is specifically used to extract the three-dimensional point cloud data representing the outline of the target area from the target three-dimensional point cloud data based on the rolling ball method, and obtain multiple boundary points corresponding to the target area.
[0155] In a possible design, the path acquisition module 401 is specifically used to: acquire a depth image including the target area; and perform coordinate transformation on each pixel point in the depth image to obtain three-dimensional point cloud data including the target area.
[0156] It should be noted that Figure 6 For the contents not mentioned in the corresponding embodiments, please refer to the description of the aforementioned method embodiments, which will not be repeated here.
[0157] The above-mentioned device obtains the original border path corresponding to the target area, the original border path includes multiple ordered boundary points of the target area, and then smoothes the original border path based on the multiple ordered boundary points on the original border path to obtain the smooth border path corresponding to the target area, the smooth border path includes multiple curved border paths connected end to end, thereby realizing local optimization of the border path, so that the local path in the border path is a smooth curve; then the path control points of the multiple curved border paths on the smooth border path are optimized, so that the adjacent two curved border paths in the multiple curved border paths are bent to the same side, and the planned border path of the target area is obtained, thereby realizing the overall optimization of the border path, and by optimizing the path control points of the multiple curved border paths on the smooth border path, so that the adjacent two curved border paths in the smooth border path are bent to the same side, the curvature of the adjacent two local paths in the border path can be reduced, showing a local path straightening effect, thereby making the overall curvature of the border path smaller and smoother, so that the overall smoothness of the border path is higher.
[0158] See also Figure 7 , Figure 7 1 is a schematic diagram of the structure of a computer device provided in an embodiment of the present application, wherein the computer device 50 includes a processor 501 and a memory 502. The memory 502 is connected to the processor 501, for example, via a bus.
[0159] The processor 501 is configured to support the computer device 50 to perform the corresponding functions in the method in the above method embodiment. The processor 501 can be a central processing unit (CPU), a network processor (NP), a hardware chip or any combination thereof. The above hardware chip can be an application specific integrated circuit (ASIC), a programmable logic device (PLD) or a combination thereof. The above PLD can be a complex programmable logic device (CPLD), a field-programmable gate array (FPGA), a generic array logic (GAL) or any combination thereof.
[0160] The memory 502 is used to store program codes, etc. The memory 502 may include a volatile memory (VM), such as a random access memory (RAM); the memory 502 may also include a non-volatile memory (NVM), such as a read-only memory (ROM), a flash memory, a hard disk drive (HDD) or a solid-state drive (SSD); the memory 502 may also include a combination of the above-mentioned types of memories.
[0161] The processor 501 may call the program code to perform the following operations:
[0162] Acquire an original edge path corresponding to a target area, wherein the target area is an area of a current edge path to be planned, and the original edge path includes a plurality of ordered boundary points of the target area;
[0163] Based on the multiple ordered boundary points, the original edge path is smoothed to obtain a smooth edge path corresponding to the target area, wherein the smooth edge path includes multiple curved edge paths connected end to end;
[0164] The path control points of the multi-segment curved edge path are optimized so that two adjacent curved edge paths in the multi-segment curved edge path bend to the same side, thereby obtaining a planned edge path of the target area. The path control points are used to control the bending direction and shape of the curved edge path.
[0165] An embodiment of the present application further provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, wherein the computer program includes program instructions, and when the program instructions are executed by a computer, the computer executes the method described in the above embodiment.
[0166] A person skilled in the art can understand that all or part of the processes in the above-mentioned embodiments can be implemented by instructing the relevant hardware through a computer program, and the program can be stored in a computer-readable storage medium, and when the program is executed, it can include the processes of the embodiments of the above-mentioned methods. The storage medium can be a disk, an optical disk, a read-only memory (ROM) or a random access memory (RAM), etc.
[0167] The above disclosure is only the preferred embodiment of the present application, which certainly cannot be used to limit the scope of rights of the present application. Therefore, equivalent changes made according to the claims of the present application are still within the scope covered by the present application.
Claims
1. A method for generating an edge path, characterized in that: include: Acquire an original edge path corresponding to a target area, wherein the target area is an area of a current edge path to be planned, and the original edge path includes a plurality of ordered boundary points of the target area; Based on the multiple ordered boundary points, the original edge path is smoothed to obtain a smooth edge path corresponding to the target area, wherein the smooth edge path includes multiple curved edge paths connected end to end; The path control points of the multi-segment curved edge path are optimized so that two adjacent curved edge paths in the multi-segment curved edge path bend to the same side, thereby obtaining a planned edge path of the target area. The path control points are used to control the bending direction and shape of the curved edge path.
2. The method according to claim 1, characterized in that The step of optimizing the path control points of the multi-segment curved edge paths so that two adjacent curved edge paths in the multi-segment curved edge paths bend toward the same side to obtain the planned edge path of the target area includes: Constructing an optimization objective function related to the path control points, wherein the optimization objective function takes the path control points as independent variables, and the optimization objective function is used to improve the overall smoothness of the path; The path control points of the multi-segment curve edge path are used as the initial solution of the independent variable in the optimization objective function, and the optimal solution of the independent variable is determined to obtain a new path control point, wherein the optimal solution minimizes the function value of the optimization objective function; The planned edge-adjacent path is generated according to the new path control point.
3. The method according to claim 2, characterized in that The optimization objective function is a nonlinear function related to the path control points.
4. The method according to claim 2, characterized in that: The optimization objective function includes a smoothness optimization objective function and a motion constraint objective function. The function value of the optimization objective function is obtained based on the function value of the smoothness optimization objective function and the function value of the motion constraint objective function. The smoothness optimization objective function is used to optimize the overall smoothness of the path, and the motion constraint function is used to constrain the speed and acceleration on the path.
5. The method according to claim 4, characterized in that The smoothness optimization objective function is: Among them, f s The function value of the objective function for smoothness optimization, Q i+1 , Q i and Q i-1 is the position vector of three adjacent path control points, and pb is the order of the curve corresponding to the curve edge path.
6. The method according to claim 4, characterized in that The motion constraint objective function includes: Among them, f v (v t ) is the function value of the speed curve function, f a (v a ) is the function value of the acceleration curve function, v max and a max are the preset maximum speed and maximum acceleration, v t and a t are the velocity and acceleration on the curve edge path, v t and a t The path control points of the curve-fitting path are obtained.
7. The method according to claim 4, characterized in that The function value of the optimization objective function is obtained by weighted summing the function value of the smoothness optimization objective function and the function value of the motion constraint objective function.
8. The method according to any one of claims 1 to 7, characterized in that: The step of smoothing the original edge path based on the plurality of ordered boundary points to obtain a smooth edge path corresponding to the target area includes: Curve fitting is performed on the plurality of ordered boundary points to obtain the smooth edge-fitting path.
9. The method according to claim 8, characterized in that The step of performing curve fitting on the plurality of ordered boundary points to obtain the smooth edge-fitting path includes: B-spline curve fitting is performed on the plurality of ordered boundary points to obtain the smooth edge-fitting path.
10. The method according to any one of claims 1 to 7, characterized in that: The obtaining of the original edge path corresponding to the target area includes: Acquire target three-dimensional point cloud data corresponding to the target area, wherein the target three-dimensional point cloud data is used to characterize the target area; Extracting the three-dimensional point cloud data representing the outline of the target area from the target three-dimensional point cloud data to obtain a plurality of boundary points corresponding to the target area; The multiple boundary points are sorted to obtain an original edge path corresponding to the target area.
11. The method according to claim 10, characterized in that The step of extracting three-dimensional point cloud data representing the outline of the target area from the target three-dimensional point cloud data to obtain a plurality of boundary points includes: Based on the rolling ball method, three-dimensional point cloud data representing the outline of the target area is extracted from the target three-dimensional point cloud data to obtain a plurality of boundary points corresponding to the target area.
12. The method according to claim 10, characterized in that The acquiring target three-dimensional point cloud data corresponding to the target area includes: Acquire three-dimensional point cloud data containing the target area; Perform target segmentation on the three-dimensional point cloud data to obtain the target three-dimensional point cloud data.
13. The method according to claim 12, characterized in that The acquiring of three-dimensional point cloud data containing the target area comprises: Acquire a depth image containing the target area; Coordinate transformation is performed on each pixel in the depth image to obtain three-dimensional point cloud data containing the target area.
14. A device for generating an edge path, characterized in that: include: A path acquisition module, used to acquire an original edge-adjacent path corresponding to a target area, wherein the target area is an area of the current edge-adjacent path to be planned, and the original edge-adjacent path includes a plurality of ordered boundary points of the target area; A path smoothing module, configured to smooth the original edge path based on the plurality of ordered boundary points to obtain a smooth edge path corresponding to the target area, wherein the smooth edge path includes a plurality of curved edge paths connected end to end; A path optimization module is used to optimize the path control points of the multi-segment curved edge path so that two adjacent curved edge paths in the multi-segment curved edge path bend to the same side to obtain the planned edge path of the target area. The path control points are used to control the bending direction and shape of the curved edge path.
15. A computer device, characterized in that: The method comprises a memory and a processor, wherein the memory and a processor are connected to the processor, and the processor is used to execute one or more computer programs stored in the memory. When the processor executes the one or more computer programs, the computer device implements the method according to any one of claims 1 to 13.
16. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, wherein the computer program includes program instructions, and when the program instructions are executed by a processor, the processor is caused to perform the method according to any one of claims 1 to 13.
17. A cleaning robot, characterized in that: The mobile cleaning robot comprises a mobile body and a cleaning robot arm, the mobile cleaning robot is used to execute the method according to any one of claims 1 to 13, and the planned edge path is the edge path of the cleaning robot arm when cleaning the target area.
Citation Information
Patent Citations
Program segment smooth compression processing method suitable for numerical control device
CN101881952A
Path planning method, computer equipment and storage medium
CN115280102A
Path Planning
US20080255728A1
Cited By
Casting cleaning robot self-adaptive cleaning method based on machine vision and trajectory planning
CN119501946A
