A method for judging the operation path points of a spraying robot moving along a wall

Abstract

The present invention relates to the technical field of spraying robots, and solves the technical problem that an operation path point at a wall corner is prone to passing through the wall or being too close to the wall. In particular, it relates to a method for determining an operation path point of a spraying robot moving along a wall, comprising the following steps: S1, obtaining the global operation path points of the spraying robot moving along the wall; S2, determining whether a single operation path point in the global operation path points passes through the wall; if so, recalculating the coordinates of the operation path point that passes through the wall and then entering step S3; if not, entering step S3. The present invention can determine whether an operation path point of the spraying robot passes through the wall or is too close to the wall, and simultaneously recalculate the coordinates of the operation path point that has this problem, thereby ensuring the validity of the operation path point and ensuring that the spraying robot can smoothly move to the designated position and perform wall spraying operations.

Description

Technical Field

[0001] The present invention relates to the technical field of spraying robots, and in particular to a method for determining operation path points of a spraying robot moving along a wall. Background Art

[0002] At present, in the building decoration market, the manual work efficiency of indoor wall spraying is low, and the paint contains a large amount of harmful chemicals such as formaldehyde and benzene that are harmful to the human body. In this case, automatic spraying robots are used to replace manual spraying operations.

[0003] In the application of fully automatic spraying robots, the operation path planning of the spraying robot determines whether the spraying operation can be carried out with intelligent automatic control. Due to the complex indoor environment of buildings, there are many corners and load-bearing columns and other building structures. Therefore, when the robot is located at the corner of the wall, the following two phenomena are likely to occur for the operation path points corresponding to the wall in this area:

[0004] First, the operation path points generated by the spraying robot based on the floor plan are prone to passing through the wall at the corner of the wall, that is, the calculated operation path points are located outside the wall;

[0005] Secondly, the generated operation path points are likely to be too close to the wall at the corners of the wall, which makes it impossible for the spray robot's robotic arm to extend.

[0006] Both of these phenomena will lead to errors in the generated operation path points, resulting in the spray robot being unable to move to the specified position when it moves to the operation path point, and being too close to the wall when working against the wall, which will cause the spray robot to be unable to continuously and fully automatically spray the interior walls of the building. Summary of the Invention

[0007] In response to the shortcomings of the existing technology, the present invention provides a method for determining the working path points of a spray robot moving along a wall, which solves the technical problem that the working path points are prone to passing through the wall or being too close to the wall at the corners of the wall.

[0008] To solve the above technical problems, the present invention provides the following technical solution: a method for determining the path points of a painting robot moving along a wall, comprising the following steps:

[0009] S1. Obtain the global operation path points of the spraying robot moving along the wall;

[0010] S2. Determine whether a single operation path point in the global operation path points passes through a wall;

[0011] If so, the coordinates of the working path points passing through the wall are recalculated and the process proceeds to step S3;

[0012] If not, go directly to step S3;

[0013] S3, determining whether a single operation path point in the global operation path point is too close to the wall;

[0014] If so, the coordinates of the working path points that are too close to the wall are recalculated and the process proceeds to step S4;

[0015] If not, go directly to step S4;

[0016] S4. Output the judgment result.

[0017] Furthermore, in step S2, the specific process includes the following steps:

[0018] S21. Select a boundary line AT with point A as the starting point and point T as the inflection point, and segment the boundary line AT into sub-segments AB and A2B2 with a step length of step;

[0019] S22, determining whether the operation path point P2 corresponding to the sub-segment A2B2 passes through the wall;

[0020] If yes, proceed to step S23;

[0021] If not, proceed to step S3;

[0022] S23: Recalculate the coordinates of the operation path point P'2.

[0023] Furthermore, in step S22, the specific process includes the following steps:

[0024] S221. Assume that the coordinates of the starting point A2 are (A2x, A2y), the coordinates of the inflection point T are (Tx, Ty), the coordinates of the operation path point P2 are (P2x, P2y), the coordinates of the point S are (Sx, Sy), and the coordinates of the point J are (Jx, Jy);

[0025] S222. Assume that the equation of the line segment A2P2 formed by the starting point A2 and the operation path point P2 is y1 = k1x1 + b1, and the equation of the line segment ST formed by the point S and the inflection point T is y2 = k2x2 + b2;

[0026] S223. Determine whether line segment A2P2 intersects line segment ST.

[0027] When k1=k2, line segment A2P2 is parallel to line segment ST and has no intersection, so the operation path point P2 does not pass through the wall;

[0028] When k1≠k2, find the coordinates of the intersection point (x0, y0) by combining the equation y1 of the line segment A2P2 and the equation y2 of the line segment ST.

[0029] If A2x≤x0≤P2x or P2x≤x0≤A2x and Tx≤x0≤Sx or Sx≤x0≤Tx and A2y≤y0≤P2y or P2y≤y0≤A2y and Ty≤y0≤Sy or Sy≤y0≤Ty, line segment A2P2 intersects line segment ST, then the operation path point P2 passes through the wall.

[0030] Furthermore, in step S23, the specific process includes the following steps:

[0031] S231, select the inflection point T as the starting point, and segment the boundary line TA into a sub-segment TA'2 with an operation step length step;

[0032] S232, calculating the coordinates of the end point A'2 in the sub-segment TA'2 according to the coordinates of the starting point T and the operation step step;

[0033] S233, calculating the coordinates of the operation path point P'2 corresponding to the sub-segment TA'2 according to the coordinates of the starting point T and the end point A'2;

[0034] S234, calculate the posture of the spraying robot at the operation path point P'2 according to the coordinates of the operation path point P'2

[0035] Furthermore, in step S232, the specific process includes the following steps:

[0036] S2321, let the coordinates of the starting point T be (x T ,y T ) and the coordinates of point A2 are

[0037] S2322, according to the coordinates of the starting point T (x T ,y T ) and the coordinates of point A2 Calculate the vector between the starting point T and the end point A'2 and vectors Length of mold

[0038] S2323, according to the vector and mold length Calculating vectors The unit vector

[0039] S2324, according to the unit vector The coordinates of the end point A'2 are obtained by the operation step step

[0040] Furthermore, in step S233, the specific process includes the following steps:

[0041] S2331. Assume that the operation path point corresponding to sub-segment TA'2 is P'2, the angle between line segment TP'2 and line segment TA'2 is θ'2, and let |TA'2| = step, where step is the operation step size; |P'2D'2| = dist, where the distance from the wall when the spraying robot faces the wall; |TD'2| = left_dist, where left_dist is the maximum distance from the wall when the spraying robot's left hand is spraying.

[0042] S2332. Take a point C'2 on the line segment TA'2 and set |TC'2| = |TP'2|.

[0043] S2333. Calculate the coordinate value of the work path point P'2 in the local coordinate system with the starting point T as the origin by using the angle θ'2 between the line segment TP'2 and the line segment TA'2;

[0044] S2334. Calculate the actual coordinates of the operation path point P'2 in the original coordinate system with point O as the origin through the coordinate transformation matrix. The actual coordinates are the coordinates of the operation path point P'2.

[0045] Furthermore, in step S3, the specific process includes the following steps:

[0046] S31. Assume that the coordinates of the work path point P3 are (P3x, P3y), the coordinates of the endpoint T are (Tx, Ty), and the coordinates of the endpoint S are (Sx, Sy);

[0047] S32. Let the equation of the line on which the line segment ST lies be A S x+B S y+C S = 0, the coordinates of the foot point H of the perpendicular line from the work path point P3 to the line segment ST are (Hx, Hy);

[0048] S33, calculate the distance d from the operation path point P3 to the straight line where the line segment ST is located PST , the distance d from the work path point P3 to the endpoint S of the line segment ST PS , the distance d from the work path point P3 to the endpoint T of the line segment ST PT , the distance d from the foot point H to the endpoint S of the line segment ST HS , the distance d from the foot point H to the endpoint T of the line segment ST HT , the distance d from the endpoint S of the line segment ST to the endpoint T of the line segment ST ST ;

[0049] S34, according to the distance d HS , distance dHT and distance d ST Select the comparison value;

[0050] If d HT +d HS >d ST , then choose the distance d PS and distance d PT The smallest value is used as the comparison value;

[0051] If d HT +d HS =d ST , then choose the distance d PST As a comparison value;

[0052] S35, compare the comparison value with the distance threshold S from the wall to_wall Make a comparison;

[0053] If the contrast value is less than the distance threshold S from the wall to_wall , then recalculate the coordinates of the new operation path point P'3.

[0054] By means of the above technical solution, the present invention provides a method for determining the path points of a painting robot moving along a wall, which has at least the following beneficial effects:

[0055] 1. The present invention can judge whether the operation path point passes through the wall, and recalculate and obtain the coordinates of the operation path point that passes through the wall, thereby avoiding the phenomenon that the spray robot cannot operate due to the operation path point passing through the wall, thereby ensuring the normal operation of the spray robot.

[0056] 2. The present invention can judge whether the operation path point is too close to the wall, and recalculate and obtain the coordinates of the operation path point that is too close to the wall, so as to avoid the phenomenon that the mechanical arm of the spray robot cannot be extended normally due to the operation path point being too close to the wall. This can ensure the normal extension operation of the mechanical arm and improve the effect and quality of automated wall spraying. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] The drawings described herein are used to provide a further understanding of the present application and constitute a part of the present application. The illustrative embodiments of the present application and their descriptions are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings:

[0058] Figure 1 This is a flow chart of the operation path point determination method of the present invention;

[0059] Figure 2 is a schematic diagram of global operation path points of an exemplary building of the present invention;

[0060] Figure 3This is a schematic diagram of the calculation of the wall penetration operation path points of the present invention;

[0061] Figure 4 This is a schematic diagram of the calculation of the path points of the operation too close to the wall in the present invention. DETAILED DESCRIPTION

[0062] To make the above-mentioned objectives, features, and advantages of the present invention more clearly understood, the present invention is further described below in detail with reference to the accompanying drawings and specific embodiments. This will enable a full understanding of how this application uses technical means to solve technical problems and achieve technical effects, and to implement the invention accordingly.

[0063] Those skilled in the art will appreciate that all or part of the steps in the above-mentioned embodiment methods can be accomplished by instructing the relevant hardware through a program. Therefore, the present application may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Furthermore, the present application may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0064] Please refer to Figure 1-Figure 4 , shows a specific implementation method of this embodiment, which solves the technical problem that the operation path point is prone to passing through the wall or being too close to the wall at the corner of the wall. It can judge whether the operation path point of the spray robot passes through the wall or is too close to the wall, and at the same time recalculate the coordinates of the operation path point with this problem, so as to ensure the validity of the operation path point and ensure that the spray robot can smoothly move to the designated position and perform wall spraying operations.

[0065] Please refer to Figure 1 This embodiment proposes a method for determining the path points of a painting robot moving along a wall, the method comprising the following steps:

[0066] S1. Obtain the global operation path points of the spraying robot moving along the wall. The global operation path points are several operation path points corresponding to all the walls in the entire building, such as Figure 2 The figure shows a schematic diagram of the global operation path points of a building, wherein the global operation path points are generated on the indoor floor plan, which is an electronic version of the CAD floor plan drawing. Figure 2 In the figure, the black frame is the wall measured in the indoor floor plan. The points on the wall are the inflection points, that is, the inner and outer corners between the walls, that is, the corner positions of the walls in the indoor floor plan. The number and position of the inflection points can be directly confirmed from the electronic version of the CAD floor plan drawing and will not be described in detail here.

[0067] like Figure 3 As shown in FIG, when the boundary line AT is sequentially divided into sub-segments, a sub-segment AB is obtained. At this time, due to the existence of the inflection point T, the length between the end point B and the inflection point T is less than the fixed operation step length step. During the division, the end point B2 of another sub-segment BB2 is located outside the wall. The operation path point P2 corresponding to the sub-segment BB2 is calculated to be located outside the wall, that is, the path point passes through the wall, as shown in FIG. Figure 3 For the operation path point P2 corresponding to the sub-segment BB2, the effective spraying area is located within the line segment BT. However, due to the existence of the inflection point T, the position of the operation path point P2 is outside the wall relative to the boundary line AT. Therefore, for this situation, this embodiment determines whether the operation path point passes through the wall and recalculates the position coordinates of the operation path point that passes through the wall, so as to avoid the position of the operation path point passing through the wall.

[0068] S2. Determine whether a single operation path point in the global operation path points passes through a wall;

[0069] If so, the coordinates of the work path points passing through the wall are recalculated and the process proceeds to step S3;

[0070] If not, go directly to step S3;

[0071] In step S2, the specific process includes the following steps:

[0072] S21. Select a boundary line AT with point A as the starting point and point T as the inflection point, and segment the boundary line AT into sub-segments AB and A2B2 with a step length of step;

[0073] The operation step length step is the area range in which the spray robot performs the spraying operation. For example, when the spray robot is at a certain operation path point, the effective operation range of the spray robot for the wall is the effective spraying area covered in all directions with the operation path point as the origin. The length of this area is the fixed operation step length. This length can be determined in combination with the design parameters of the spray robot and the actual situation on site, or it can be set by technical personnel, so it will not be described in detail here.

[0074] S22, determining whether the operation path point P2 corresponding to the sub-segment A2B2 passes through the wall;

[0075] If yes, proceed to step S23;

[0076] If not, go to step S3.

[0077] like Figure 3As shown, if the operation path point passes through the wall, it means that the line connecting the starting point A2 of the segmented sub-segment and the operation path point P2 must intersect with the line connecting segment ST at a point J. The coordinates of the intersection are obtained by combining the equation of the straight line y1 where segment A2P2 is located and the equation of the straight line y2 where segment ST is located. It is observed whether the coordinates of the intersection are both within the coordinates of the endpoints of segment A2P2 and segment ST to determine whether segment A2P2 intersects with segment ST, that is, whether the path point passes through the wall. Then, a new segmentation segment is selected for the path point that passes through the wall to recalculate the operation path point.

[0078] In step S22, the specific process includes the following steps:

[0079] S221. Assume that the coordinates of the starting point A2 are (A2x, A2y), the coordinates of the inflection point T are (Tx, Ty), the coordinates of the operation path point P2 are (P2x, P2y), the coordinates of the point S are (Sx, Sy), and the coordinates of the point J are (Jx, Jy);

[0080] S222. Assume that the equation of the line segment A2P2 formed by the starting point A2 and the operation path point P2 is y1 = k1x1 + b1, and the equation of the line segment ST formed by the point S and the inflection point T is y2 = k2x2 + b2;

[0081] S223. Determine whether line segment A2P2 intersects line segment ST.

[0082] When k1=k2, line segment A2P2 is parallel to line segment ST and has no intersection, so the operation path point P2 does not pass through the wall;

[0083] When k1≠k2, find the coordinates of the intersection point (x0, y0) by combining the equation y1 of the line segment A2P2 and the equation y2 of the line segment ST.

[0084] The calculation formula for the coordinates of the intersection point (x0, y0) is:

[0085]

[0086] In the above formula, k1 and b1 respectively represent the slope and intercept of the straight line equation y1 where the line segment A2P2 is located, and k2 and b2 respectively represent the slope and intercept of the straight line equation y2 where the line segment ST is located.

[0087] If A2x≤x0≤P2x or P2x≤x0≤A2x and Tx≤x0≤Sx or Sx≤x0≤Tx and A2y≤y0≤P2y or P2y≤y0≤A2y and Ty≤y0≤Sy or Sy≤y0≤Ty, line segment A2P2 intersects line segment ST, then the operation path point P2 passes through the wall and the process goes to step S23;

[0088] S23: Recalculate the coordinates of the operation path point P'2.

[0089] In step S23, the specific process includes the following steps:

[0090] S231, select the inflection point T as the starting point, and divide the boundary line TA into sub-segments TA'2 with the operation step length step; according to The direction and the operation step length step of the spraying operation determine the end point A'2 of the sub-segment, thereby dividing the sub-segment TA'2.

[0091] S232, calculate the coordinates of the end point A'2 in the sub-segment TA'2 according to the coordinates of the starting point T and the operation step step; here, the coordinates of the starting point T are known coordinates, which are (x T ,y T ), so the coordinates of the end point A'2 can be obtained by the known coordinates of the starting point T and the operation step length step;

[0092] In step S232, the specific process includes the following steps:

[0093] S2321, let the coordinates of the starting point T be (x T ,y T ) and the coordinates of point A2 are

[0094] S2322, according to the coordinates of the starting point T (x T ,y T ) and the coordinates of point A2 Calculate the vector between the starting point T and the end point A'2 and vectors Length of mold

[0095] Specifically, vector and mold length The calculation formula is:

[0096]

[0097]

[0098] S2323, according to the vector and mold length Calculating vectors The unit vector

[0099] Specifically, the unit vector The calculation formula is:

[0100]

[0101] S2324, according to the unit vector The coordinates of the end point A'2 are obtained by the operation step step

[0102] Specifically, the coordinates of the end point A'2 for:

[0103]

[0104]

[0105] In the above formula, |TA'2|=step, which is the operation step size.

[0106] S233, calculating the coordinates of the operation path point P'2 corresponding to the sub-segment TA'2 according to the coordinates of the starting point T and the end point A'2;

[0107] In step S233, the specific process includes the following steps:

[0108] S2331. Assume that the operation path point corresponding to sub-segment TA'2 is P'2, the angle between line segment TP'2 and line segment TA'2 is θ'2, and let |TA'2| = step, where step is the operation step size; |P'2D'2| = dist, where the distance from the wall when the spraying robot faces the wall; |TD'2| = left_dist, where left_dist is the maximum distance from the wall when the spraying robot's left hand is spraying.

[0109] The calculation formula for the angle θ'2 between line segment TP'2 and line segment TA'2 is:

[0110]

[0111] S2332. Take a point C'2 on the line segment TA'2 and set |TC'2| = |TP'2|.

[0112] Specifically, the specific position of point C'2 on sub-segment TA'2 can be determined, and the length of |TP'2| can be calculated using the following formula:

[0113]

[0114] Specifically, the coordinates of point C'2 for:

[0115]

[0116]

[0117] In the above formula, x T and y T are the coordinates of the starting point T, It is the unit vector from the starting point T to the end point A’2.

[0118] S2333. Calculate the coordinate value of the operation path point P’2 in the local coordinate system with the starting point T as the origin through the included angle θ’2 between the line segment TP’2 and the line segment TA’2;

[0119] As Figure 3 shown, first establish a spraying robot coordinate system with the spraying robot itself as the origin at the operation path point P’2, and the direction of the vector as the positive x-axis direction. Use the included angle between the positive x-axis direction of the spraying robot coordinate system and the positive x-axis direction of the original coordinate system with point O as the origin as the attitude representation of the spraying robot at the operation path point P’2. Then establish a local coordinate system with the starting point T as the origin and the direction of the vector as the positive x-axis direction.

[0120] Let and respectively represent the rotation matrix, translation matrix, and transformation matrix when the local coordinate system with the starting point T as the origin rotates to the original coordinate system with point O as the origin. and respectively represent the coordinate values of the operation path point P’2 in the original coordinate system with O as the origin. Tx and Ty respectively represent the coordinate values in the local coordinate system with the starting point T as the origin, and Sx and Sy respectively represent the coordinate values in the coordinate system with point S as the origin.

[0121] α’ is the included angle between the positive x-axis direction of the local coordinate system with the starting point T as the origin and the positive x-axis direction of the original coordinate system with point O as the origin. If Tx = Sx and Ty > Sy, then α’ = -π / 2; if Tx = Sx and Ty < Sy, then α’ = π / 2; if Tx ≠ Sx, then α’ = atan2(Sy - Ty, Sx - Tx). From Figure 3 it can be seen that the coordinate of the operation path point P’2 in the local coordinate system with the starting point T as the origin is:

[0122]

[0123]

[0124] In the above formula, |TC’2| = |TP’2|, and |TP’2| is obtained from step S5432.

[0125] S2334. Calculate the actual coordinate of the operation path point P’2 in the original coordinate system with point O as the origin through the coordinate transformation matrix. The actual coordinate is the coordinate of the operation path point P’2;

[0126] The rotation matrix for converting the local coordinate system with the starting point T as the coordinate origin to the original coordinate system with the point O as the origin Translation matrix And the transformation matrix Are respectively:

[0127]

[0128]

[0129]

[0130] Thus, the coordinate transformation relationship of the operation path point P’2 in the two coordinate systems can be expressed as That is:

[0131] ]>

[0132]

[0132] By calculation, it can be obtained that:

[0133] P’2 O x = |TC’2|cos(π / 2 + α’ - θ’2) + Tx

[0134] P’2 O y = |TC’2|sin(π / 2 + α’ - θ’2) + Ty

[0135] Since the calculation methods of the included angles α’ and θ’2 are given, the coordinates of the operation path point P’2 in the original coordinate system with the point O as the origin can be determined as:

[0136] (|TC’2|cos(π / 2 + α’ - θ’2) + Tx, |TC’2|sin(π / 2 + α’ - θ’2) + Ty)

[0137] Thus, the calculation of the coordinates of the operation path point P’2 corresponding to the sub-segment TA’2 is completed when the wall-piercing phenomenon occurs at the operation path point.

[0138] S2335. Calculate the posture of the spraying robot when operating at the operation path point P’2

[0139] S234. Calculate the posture of the spraying robot when operating at the operation path point P’2 according to the coordinates of the operation path point P’2

[0140] Since the positive direction of the x-axis in the spraying robot coordinate system is consistent with the Figure 3 in direction, if Tx = A’2x and Ty > A’2y, then If Tx = A’2x and Ty < A’2y, then If Tx≠A'2x, then

[0141] Through the algorithm provided in this embodiment, it is possible to judge whether the work path point passes through the wall, and recalculate and obtain the coordinates of the work path point that passes through the wall, thereby avoiding the phenomenon that the spray robot cannot operate due to the work path point passing through the wall. This can ensure the normal operation of the spray robot and improve the effect and quality of automated wall spraying.

[0142] like Figure 4 As shown in FIG, when the boundary line AT is sequentially divided into several sub-segments, the operation path point P3 may encounter a situation where the spraying robot is too close to the wall, which may cause the robot arm of the spraying robot to fail to work normally. Figure 4 As shown in the working path point P3 in the figure, the position of this point is too close to the wall (line segment ST). It can be understood that the distance between the spray robot and the wall is too close. The robotic arm cannot be extended normally, resulting in the spray robot being unable to operate normally in this special case. That is, the line segment |P3H| perpendicular to the line segment |ST| is less than the set distance threshold S from the wall. to_wall , the effective spraying area is within line segment A3T. To address this situation, this embodiment determines whether the operation path point is too close to the wall and recalculates the position coordinates of the path point that is too close to the wall, so that the position of the operation path point does not appear to be too close to the wall.

[0143] S3, determining whether a single operation path point in the global operation path point is too close to the wall;

[0144] If so, the coordinates of the working path points that are too close to the wall are recalculated and the process proceeds to step S4;

[0145] If not, go directly to step S4;

[0146] In this step, it is determined whether a single operation path point in the global operation path point is too close to the wall. The main method is to first calculate the distance from the operation path point P3 to the straight line where the line segment ST is located and the coordinates of the foot of the perpendicular, and determine whether the foot of the perpendicular is located on the line segment ST. If the foot of the perpendicular is located outside it, the distances from the operation path point P3 to the two end points of the line segment ST are calculated respectively, and the smaller distance is selected as the distance from the operation path point P3 to the line segment ST. If this distance is less than the distance threshold S from the wall, to_wall , then the coordinates of the new operation path point P'3 are calculated according to the method of recalculating the operation path point P'2 in step S23.

[0147] In step S3, the specific process includes the following steps:

[0148] S31. Assume that the coordinates of the work path point P3 are (P3x, P3y), the coordinates of the endpoint T are (Tx, Ty), and the coordinates of the endpoint S are (Sx, Sy);

[0149] S32. Let the equation of the line on which segment ST lies be A S x+B S y+C S = 0, the coordinates of the foot point H of the perpendicular line from the work path point P3 to the line segment ST are (Hx, Hy);

[0150] Specifically, the coordinates (Hx, Hy) of the perpendicular point H are expressed as:

[0151] Hx=(B S *B S *P3x-A S *B S *P3y-A S *C S ) / (A S *A S +B S *B S )

[0152] Hy=(-A S *B S *P3x+A S *A S *P3y-B S *C S ) / (A S *A S +B S *B S )

[0153] In the above formula, A S 、B S 、C S They respectively represent the parameter terms in the equation of the straight line where the line segment ST is located.

[0154] S33, calculate the distance d from the operation path point P3 to the straight line where the line segment ST is located PST , the distance d from the work path point P3 to the endpoint S of the line segment ST PS , the distance d from the work path point P3 to the endpoint T of the line segment ST PT , the distance d from the foot point H to the endpoint S of the line segment ST HS , the distance d from the foot point H to the endpoint T of the line segment ST HT , the distance d from the endpoint S of the line segment ST to the endpoint T of the line segment ST ST ;

[0155]

[0156]

[0157]

[0158]

[0159]

[0160]

[0161] S34, according to the distance d HS , distance d HT and distance d ST Select the comparison value;

[0162] If d HT +d HS >d ST , then choose the distance d PS and distance d PT The smallest value is used as the comparison value;

[0163] If d HT +d HS =d ST , then choose the distance d PST As a comparison value;

[0164] S35, compare the comparison value with the distance threshold S from the wall to_wall Make a comparison;

[0165] If the contrast value is less than the distance threshold S from the wall to_wall , then recalculate the coordinates of the new operation path point P'3;

[0166] In this step, the coordinates of the new operation path point P'3 are recalculated based on the same algorithm as that used to recalculate the coordinates of the operation path point P'2 in step S23. Figure 4 , the calculation process is as follows:

[0167] Select endpoint T as the starting point of the new line segment, according to the vector The endpoint A'3 of the new segment is calculated based on the direction and operation step length step, so that the line segment |TA'3| = step, |P'3D'3| = dist, where dist is the distance from the wall when the spraying robot is facing the wall, |TD'3| = left_dist, where left_dist is the maximum distance from the wall when the spraying robot's left hand is spraying, and the angle θ'3 between the line segment TP'3 and the line segment TC'3 is:

[0168]

[0169]

[0170] The coordinates of the starting point T are known to be (x T ,y T ), select a point C'3 on the line segment TA'3 so that |TC'3| = |TP'3|, and the coordinates of the starting point A3 of the segment are The vector from the starting point T to the starting point A3 is vector Length of mold for:

[0171]

[0172] vector The unit vector for:

[0173]

[0174] The coordinates of the new end point A'3 are for:

[0175]

[0176]

[0177] Coordinates of point C'3 for:

[0178]

[0179]

[0180] like Figure 4 As shown, first establish the vector at the operation path point P'3 with the spray robot itself as the origin The direction of the spray robot coordinate system is the positive direction of the x-axis, and the angle between the positive direction of the x-axis of the spray robot coordinate system and the positive direction of the x-axis of the original coordinate system with point O as the origin is As the posture representation of the spraying robot at the working path point P'3, we then establish the starting point T as the origin and the vector The direction of the local coordinate system is the positive direction of the x-axis, let and They represent the rotation matrix, translation matrix, and transformation matrix respectively when the local coordinate system with T as the origin is rotated to the original coordinate system with O as the origin. and They represent the coordinate values of the operation path point P'3 in the original coordinate system with O as the origin, Tx and Ty represent the coordinate values in the local coordinate system with the starting point T as the origin, and Sx and Sy represent the coordinate values in the coordinate system with point S as the origin.

[0181] α” is the angle between the positive x-axis direction of the local coordinate system with the starting point T as the origin and the positive x-axis direction of the original coordinate system with the O point as the origin. If Tx = Sx and Ty > Sy, then α” = -π / 2; if Tx = Sx and Ty < Sy, then α” = π / 2; if Tx ≠ Sx, then α” = atan2(Sy - Ty, Sx - Tx). From Figure 4 it can be seen that the coordinates of the operation path point P’3 in the local coordinate system with the starting point T as the origin are:

[0182]

[0183]

[0184] In the above formula, |TC’3| = |TP’3|.

[0185] The rotation matrix for converting the local coordinate system with the starting point T as the coordinate origin to the original coordinate system with the point O as the origin The translation matrix and the transformation matrix are respectively:

[0186]

[0187]

[0188]

[0189] Thus, the coordinate transformation relationship of the operation path point P’3 in the two coordinate systems can be expressed as That is:

[0190]

[0191] Through calculation, it can be obtained that:

[0192] P’3 O x = |TC’3|cos(π / 2 + α” - θ’3) + Tx

[0193] P’3 O y = |TC’3|sin(π / 2 + α” - θ’3) + Ty

[0194] Since the calculation methods of the included angles α” and θ’3 have been given, the coordinates of the operation path point P’3 in the original coordinate system with the O point as the origin can be determined as:

[0195] (|TC’3|cos(π / 2 + α” - θ’3) + Tx, |TC’3|sin(π / 2 + α” - θ’3) + Ty)

[0196] Thus, when the phenomenon of being too close to the wall appears at the operation path point, the calculation of the coordinates of the operation path point P'3 corresponding to the sub-segment TA'3 is completed.

[0197] Finally, it is necessary to calculate the posture of the spraying robot when operating at the operation path point P'3. Since the positive direction of the x-axis of the spraying robot coordinate system is the same as that in Figure 4 in If Tx = A'3x and Ty > A'3y, then If Tx = A'3x and Ty < A'3y, then If Tx ≠ A'3x, then

[0198] Through the algorithm provided in this embodiment, it is possible to judge whether there is a phenomenon of being too close to the wall at the operation path point, recalculate the coordinates of the operation path point that is too close to the wall and obtain them, avoiding the phenomenon that the robotic arm of the spraying robot cannot be normally extended for operation due to the operation path point being too close to the wall. Thus, it is possible to ensure the normal extension operation of the robotic arm and improve the effect and quality of automatic wall spraying.

[0199] S4. Output the judgment result, and the judgment result includes whether a single operation path point passes through the wall or is too close to the wall and the coordinates of the recalculated operation path point.

[0200] The above embodiments have introduced the present invention in detail. Specific examples are used in this article to elaborate on the principle and implementation manner of the present invention. The description of the above embodiments is only used to help understand the method and its core idea of the present invention; at the same time, for those of ordinary skill in the art, according to the idea of the present invention, there will be changes in the specific implementation manner and application scope. In summary, the content of this specification should not be construed as a limitation to the present invention.

Claims

1. A method for determining the path points of a painting robot moving along a wall, characterized in that: The following steps are involved: S1. Obtain the global operation path points of the spraying robot moving along the wall; S2. Determine whether a single operation path point in the global operation path points passes through a wall, including: S21, select a boundary line AT with point A as the starting point and point T as the inflection point, and adjust the boundary line AT with the operation step length Split into sub-segment AB and sub-segment ; S22. Judgment and sub-segments Corresponding operation path points Whether it passes through the wall; If yes, proceed to step S23; If not, proceed to step S3; S23, recalculate the operation path point coordinates of If so, the coordinates of the working path points passing through the wall are recalculated and the process proceeds to step S3; If not, go directly to step S3; S3. Determine whether a single operation path point in the global operation path point is too close to the wall, including: S31. Set operation path points The coordinates are , the coordinates of the endpoint T are , the coordinates of the endpoint S are ; S32. Let the equation of the line on which the line segment ST lies be , operation path point The coordinates of the foot point H of the perpendicular line to the line segment ST are ; S33, calculate the operation path points respectively The distance to the line segment ST , operation path points Distance to endpoint S of line segment ST , operation path points Distance to endpoint T of line segment ST , the distance from the foot point H to the endpoint S of the line segment ST , the distance from the foot point H to the endpoint T of the line segment ST , the distance between the endpoint S of the line segment ST and the endpoint T of the line segment ST ; S34, according to distance ,distance and distance Select the comparison value; like , then select the distance and distance The smallest value is used as the comparison value; like , then select the distance As a comparison value; S35, compare the comparison value with the distance threshold from the wall Make a comparison; If the contrast value is less than the distance threshold from the wall , then recalculate the new operation path point coordinates of If so, the coordinates of the working path points that are too close to the wall are recalculated and the process proceeds to step S4; If not, go directly to step S4; S4. Output the judgment result.

2. The method for determining path points of a wall-traveling operation according to claim 1, characterized in that: In step S22, the specific process includes the following steps: S221, set the starting point The coordinates are , the coordinates of the inflection point T are , operation path points The coordinates are The coordinates of point S are The coordinates of point J are ; S222, set the starting point and operation path points The line segments The equation of the straight line is The equation of the straight line where the line segment ST formed by point S and inflection point T is ; S223, judging line segments Whether it intersects with line segment ST; when When the line segment If it is parallel to line segment ST and has no intersection, then the operation path point Not through the wall; when When the simultaneous line segments Equation of the straight line Equation of the straight line containing segment ST Find the coordinates of the intersection point ; like or and or and or and or , line segment Intersects with line segment ST, then the operation path point Through walls.

3. The method for determining path points of a wall-traveling operation according to claim 1, wherein: In step S23, the specific process includes the following steps: S231, select the inflection point T as the starting point, and the boundary line TA is Split out sub-segments ; S232, according to the coordinates of the starting point T and the operation step Calculate sub-segments Intermediate endpoint coordinates of S233, according to the starting point T and the end point Coordinate calculation and sub-segment Corresponding operation path points coordinates of S234, according to the operation path point Coordinate calculation of the spraying robot at the working path point Working posture .

4. The method for determining path points of a wall-traveling operation according to claim 3, wherein: In step S232, the specific process includes the following steps: S2321. Assume the coordinates of the starting point T are and point The coordinates are ; S2322, according to the coordinates of the starting point T and point Coordinates Calculate the starting point T to the end point Vector between and vectors Length of mold ; S2323, according to the vector and mold length Calculating vectors The unit vector ; S2324, according to the unit vector and job step Get the end point The coordinates are .

5. The method for determining path points of a wall-traveling operation according to claim 3, wherein: In step S233, the specific process includes the following steps: S2331, let and sub-segment The corresponding operation path point is , line segment With line segment The angle is ,make , is the operation step length; , is the distance from the wall when the spray robot is facing the wall; , The maximum distance between the left hand of the spray robot and the wall during spraying operation; S2332, in the sub-segment Take a little bit from the top ,make ; S2333, through the line segment With line segment The angle is Calculate operation path points Coordinate values in the local coordinate system with the starting point T as the origin; S2334. Calculate the operation path points through the coordinate transformation matrix The actual coordinates in the original coordinate system with point O as the origin are the actual coordinates of the operation path point. 's coordinates.

Images