Path planning method for obstacle avoidance of construction robot arm

Abstract

The invention discloses a path planning method for obstacle avoidance of a construction mechanical arm, comprising: establishing a three-dimensional mathematical model of the movement of the construction mechanical arm, including a group of equations of the movement trajectory of the construction mechanical arm; instantiating and assigning values ​​to the mathematical model; based on the instantiated data, obtaining { α ( t ), β 1( t ), β 2( t ), β 3( t ), β 4( t )}, satisfying the arm from the starting point P1 5 To the end point P2 5 During the movement, the height of the boom frame at the plane where the obstacle is located is greater than the height of the obstacle. This method establishes a group of equations for the motion trajectory of the construction machinery boom, and achieves obstacle avoidance by obtaining an expression that satisfies the obstacle avoidance conditions, which reduces manual control and improves the automation and accuracy of the boom obstacle avoidance. This method also obtains the boom obstacle avoidance solution space through a machine learning algorithm, and also performs optimal path planning to enable the boom to complete obstacle avoidance in a short distance and with high efficiency and reach the target position.

Description

Technical Field

[0001] The invention relates to a path planning method for obstacle avoidance of a construction mechanical arm, and belongs to the technical field of engineering equipment. Background Art

[0002] In civil engineering construction, the movement control of construction machinery arms such as concrete placing booms, precast concrete structure hoisting booms or other similar booms in the construction space still relies on manual experience to operate the work proficiently. Some units in the existing technology have also developed methods of manual remote visual control, but manual control is still required, and there are problems such as low level of informatization and intelligence, and low efficiency.

[0003] Regarding the arm's obstacle avoidance and path planning, the research progress at home and abroad is basically the same, but most of them remain in the theoretical research stage. With the application of robot technology, more and more construction machinery arms will be upgraded to intelligent ones. At present, it is urgent to solve the problem of arm obstacle avoidance path planning. Summary of the invention

[0004] The invention provides a path planning method for obstacle avoidance of a construction machinery arm, which is used to solve the problems of low information and intelligence level, low efficiency and the like of the existing construction machinery arm in construction applications.

[0005] In order to solve the above technical problems, the present invention includes the following technical solutions:

[0006] A path planning method for obstacle avoidance of a construction robot arm comprises the following steps:

[0007] Step 1: Establish a three-dimensional mathematical model of the motion of the construction mechanical arm, including establishing a set of motion trajectory equations of the construction mechanical arm;

[0008] The three-dimensional cylindrical coordinate system is established with the arm support rotation center as the coordinate system origin O and the vertical upward direction as the positive direction of the z-axis. The azimuth angle of the arm is recorded as α ; The construction machine arm has M arm sections, and the lengths are R 1 , R 2 , …, R M ; The angle between the first boom section and the horizontal plane is recorded as β 1 , No. i Section Boom Section i -1 The angle between the arm sections is recorded as β i ,i=2,3,…,M;with time parameter t Establish the motion trajectory equations of the construction robot arm:

[0009] ;

[0010] In the formula, P 1 , P 2 , …, P M Respectively represent the root nodes of the 1st, 2nd, ..., Mth arm segments, P M+1 is the end node of the Mth arm segment, x 1 , z 1 Indicates the offset coordinates of the first boom section root node relative to the origin O in the boom elevation;

[0011] Step 2: Assign values ​​to the mathematical model, determine the boom parameter M, and R 1 , R 2 , …, R M The value of determines the obstacle point set and the starting point of the end node movement of the Mth arm section of the boom. and end point ;

[0012] Step 3: Based on the instantiation data, get { α ( t ), β 1 ( t ), β 2 ( t ), β 3 ( t ), β 4 ( t )}, satisfying the arm from the starting point To the end During the movement, the height of the boom frame at the plane where the obstacle is located is greater than the height of the obstacle.

[0013] Furthermore, in step 1, a three-dimensional mathematical model of the motion of the construction robot arm is established, which also includes establishing a mathematical expression for obstacles.

[0014] If the obstacle is a vertical object, it is simplified as a vertical cylinder; if the obstacle is a long strip, it is divided into several obstacles that envelop the original object;

[0015] The obstacle is described as a set of points, {( α obsi , ρ obsi ,z obsi , r obsi )}, i =1,2,…;

[0016] In the formula ρ is the distance from the point on the building robot arm to the origin O, α obsi , ρ obsi , z obsi , r obsi They represent the obstacle centroid azimuth, polar distance, vertex height, and envelope radius respectively;

[0017] In step 2, the obstacle point set is determined, specifically: according to the actual situation of the construction site, the parameters are determined α obsi , ρ obsi , z obsi , r obsi .

[0018] Further, in step 3, the height of the boom frame at the plane where the obstacle is located is greater than the height of the obstacle, which is expressed as:

[0019] ;

[0020] Where △ z Indicates the safety margin reserved in the height direction. Pl represents the spatial position of the building robot arm, { Pl ( P 1 , P 2 ,…, P M , P M+1 )}, Pl i Represents a line segment P i P i+1 .

[0021] Furthermore, step one also includes establishing that the construction machine arm has an active boundary, and the arm active boundary condition is expressed as:

[0022] ,

[0023] ,

[0024] In the formula, Indicates the azimuth of the boom α=α ex The pole distance of all parts on the boom, Indicates azimuth α=α ex The pole distance of the active boundary, Indicates the height coordinates of all parts of the boom, Z 0 Indicates the height coordinate of the ground or other platform surface, △ ρ, △ z Respectively represent the reserved safety margins in plane and height directions;

[0025] Step 2 also includes instantiating and assigning values ​​to the active boundaries of the building robot arm:

[0026] ρ ex =ρ ex ( α ex ), α ex ∈[0,2π],

[0027] z 0 = z ( α , ρ ),( α , ρ )∈Ω,

[0028] Wherein, Ω represents the movable area of ​​the frame.

[0029] Further, in step 3, based on the instantiation data, we get { α ( t ), β 1 ( t ), β 2 ( t ), β 3 ( t ), β 4 ( t )}, satisfying the arm from the starting point To the end During the movement, the height of the boom frame at the plane where the obstacle is located is greater than the height of the obstacle, specifically:

[0030] Generate a training sample set for the machine learning algorithm based on instantiated data;

[0031] Use samples to train the machine learning algorithm, and use the trained algorithm function to solve the arm obstacle avoidance solution space;

[0032] The optimal path planning is performed for the end motion of the construction robot arm in the arm obstacle avoidance solution space.

[0033] Furthermore, M=4; in step 3, a machine learning algorithm training sample set is generated based on the instantiated data, specifically including:

[0034] For all α ∈[0,2π), β 1 ∈[0,π / 2), β 2 ∈(-π,0), β 3 ∈(0,π), β 4 ∈(-π,0), preset a precision of 1 / N 0 , N 0 is a positive integer such that:

[0035] ,

[0036] ,

[0037] ,

[0038] ,

[0039] ,

[0040] in, i , j , k , l , m =0,1,2,…, N 0 ;

[0041] Create a training sample set:

[0042] { α i , β 1j , β 2k , β 3l , β 4m , y ijklm}, i , j ,k , l , m= 0,1,2,…, N 0 ,

[0043] The sample size is ( N 0 +1) 5 The classification label of each sample y ijklm , determined according to the following rules:

[0044] .

[0045] Furthermore, in step three, the machine learning algorithm is trained with samples, and the trained algorithm function is used to solve the arm obstacle avoidance solution space, which specifically includes:

[0046] The sample training machine learning algorithm adopts the support vector machine algorithm;

[0047] For the established training sample set, a nonlinear kernel function and appropriate parameters are selected to train the support vector machine algorithm. The training generates a support vector machine algorithm function, which is recorded as:

[0048] y = F ( α , β 1 , β 2 , β 3 , β 4 ),

[0049] Solve the obstacle avoidance solution space for all α ∈[0,2π), β 1 ∈[0,π / 2), β 2 ∈(-π,0), β 3 ∈(0,π), β 4 ∈(-π,0), such that:

[0050] y = F ( α , β 1 , β 2 , β 3 , β 4 )=1,

[0051] ( α , β 1 , β 2 , β 3 , β 4 ) The arm can be within the range of the activity boundary and actively avoid all obstacles, and the obstacle avoidance solution space of the frame is obtained as follows:

[0052] .

[0053] Furthermore, in step three, the optimal path planning is performed for the end motion of the construction machinery arm in the arm obstacle avoidance solution space. Specifically, in the obstacle avoidance solution space, the shortest motion path is taken as the goal, and the dynamic programming algorithm is used to perform the optimal path planning for the end motion. The algorithm implementation steps are as follows:

[0054] A1. Set an optimal path point set first , used to record the optimal path planned by the algorithm, where N The value of is to be determined, satisfying:

[0055] ,

[0056] And there are:

[0057] ;

[0058] A2. Set another starting point and end point Dynamic adjustment point between , The initial value is set to ;

[0059] Dynamically update each Before, dynamically determine The coordinate position of Coordinate location basis To the end Whether it is determined by visual communication or not, the rules are as follows:

[0060] a. If To the end If the Dynamically adjust to ;

[0061] b. If To the end No line of sight, then, for the straight line The point set on the active boundary of the arm's active range , existence point P ex0 ( α ex0 , ρ ex0 , z 0 ) makes the angle , Dynamically adjust to P ex0 ( α ex0 , ρ ex0 , z 0 );

[0062] A3. Dynamic update under conditions ;

[0063] Considering the influence of the fence, each moving step will Towards Specific direction Move one displacement control accuracy△ S arrive , and each mobile step arm is in the obstacle avoidance solution space within the fence activity space, expressed as:

[0064] ,

[0065] ;

[0066] In the formula, express The unit vector of the direction, ;△ S The minimum moving distance for the end motion control of the boom;

[0067] ;

[0068] △ d It represents the accuracy of the motion control of the end of the boom, which is a constant;

[0069] Solve and record updates middle;

[0070] A4. Order n=n+ 1, repeat A2 and A3 until ; Solve and record the update entry middle;

[0071] A5. The equation set { α (t ), β 1 ( t ), β 2 ( t ), β 3 ( t ), β 4 ( t )}.

[0072] Due to the adoption of the above technical solution, the present invention has the following advantages and positive effects compared with the prior art: the path planning method for obstacle avoidance of a construction machinery arm provided by the present invention establishes a group of equations for the motion trajectory of the construction machinery arm, and after assigning values ​​to the group of equations, simplifies the group of equations into a arm structure with M+1 degrees of freedom, and obtains { α ( t ), β 1 ( t ), β 2 ( t ), β 3 ( t ), β 4 ( t )} expression is used to achieve obstacle avoidance, reduce manual control, and improve the automation and accuracy of boom obstacle avoidance. In addition, the method also provides an algorithm function obtained by training samples through a machine learning algorithm, which can solve the boom obstacle avoidance solution space, and also obtains the optimal path by planning the optimal path of the end movement of the construction machinery boom in the boom obstacle avoidance solution space, so as to achieve the boom to complete obstacle avoidance in a short distance and with high efficiency and reach the target position. Compared with the traditional manually operated obstacle avoidance process, this method can reduce the training time of technical personnel and the input of mechanical fuel, and has significant advantages such as high construction efficiency and high level of intelligent informationization and digitization. BRIEF DESCRIPTION OF THE DRAWINGS

[0073] Figure 1 It is a flow chart of a path planning method for obstacle avoidance of a construction robot arm in one embodiment of the present invention;

[0074] Figure 2 A schematic diagram of a three-dimensional mathematical model of the motion of a construction robot arm in one embodiment of the present invention;

[0075] Figure 3 A schematic diagram of a model of a construction mechanical arm frame in one embodiment of the present invention;

[0076] Figure 4A schematic diagram of the movable boundary of a construction robot arm in one embodiment of the present invention;

[0077] Figure 5 is a schematic diagram of an obstacle model in an embodiment of the present invention;

[0078] Figure 6 The figure is a flow chart of a dynamic programming algorithm for boom end motion in one embodiment of the present invention. DETAILED DESCRIPTION

[0079] The following is a further detailed description of the path planning method for obstacle avoidance of a construction robot arm provided by the present invention in conjunction with the accompanying drawings and specific embodiments. The advantages and features of the present invention will become clearer in conjunction with the following description. It should be noted that the accompanying drawings are all in a very simplified form and are not in precise proportions, and are only used to conveniently and clearly assist in explaining the purpose of the embodiments of the present invention.

[0080] The path planning method for obstacle avoidance of a construction robot arm provided in this embodiment can be used for active obstacle avoidance of construction robot arms such as concrete placing machine arms, precast concrete structure hoisting machine arms or other similar arms, and to optimize the motion path of the end of the arm, for example, by using the shortest time or the best indicators such as safety or energy saving for optimization.

[0081] like Figure 1 As shown, the path planning method for building robot arm obstacle avoidance specifically includes the following steps:

[0082] Step 1: Establish a three-dimensional mathematical model of the motion of the construction mechanical arm, including establishing a set of motion trajectory equations of the construction mechanical arm;

[0083] Step 2: instantiate and assign values ​​to the mathematical model;

[0084] Step 3: Based on the instantiated data, a simplified parameter expression of the motion trajectory equations of the construction robot arm that meets the obstacle avoidance conditions is obtained.

[0085] 1. Further describe step 1.

[0086] like Figure 2 As shown, a 3D mathematical model of the motion of the building mechanical arm is established. Conventional concrete placing crane mechanical arm is usually a vertical folding arm that rotates around a certain center. Combined with the engineering structure plan, the arm support rotation center is taken as the origin O of the coordinate system, the vertical upward direction is the positive direction of the z axis, and the two mutually perpendicular outward directions in the horizontal plane are the positive directions of x and y respectively. A three-dimensional rectangular coordinate system is established Oxyz . A three-dimensional cylindrical coordinate system can also be used for modeling. x The angle between the axes is the azimuth α , counterclockwise is the positive direction, point ( x ,y , z ) to the horizontal distance from the origin O (projected to xO The distance in the plane is the pole distance ρ , the transformation relationship between the three-dimensional rectangular coordinate system and the three-dimensional cylindrical coordinate system is:

[0087] .

[0088] The construction robot boom has M arm sections. Let's take a 4-arm folding concrete placing boom (5 degrees of freedom) as an example. Figure 3 As shown in the figure, it can be simplified into a robot structure with 4 degrees of freedom (joint angle) of vertical arm and 1 degree of freedom (rotation angle).

[0089] Spatial location of the building mechanical arm Pl Available at each joint node P i The spatial coordinates of are determined and expressed as:

[0090] { Pl ( P 1 , P 2 , P 3 , P 4 , P 5 )};

[0091] in P 1 , P 2 , P 3 , P 4 Respectively represent the root nodes (head end nodes) of the 1st, 2nd, 3rd, and 4th arm sections. P 5 It is the end node of the 4th boom section. It is expressed as follows according to the DH rule:

[0092] ，

[0093] ，

[0094] ，

[0095] ，

[0096] ，

[0097] in:( x 1 ,0, z 1 ) represents the offset coordinate of the root node of the first boom section relative to the origin O (rotation center) in the vertical plane of the boom. R 1 , R 2 , R 3 , R 4 Respectively represent the lengths of the 1st, 2nd, 3rd, and 4th boom sections, [ x , y , z ,1] indicates x , y , z Coordinates and 1 augmented vector; [] T represents the vector transpose. They are:

[0098] ,

[0099] ,

[0100] ,

[0101] ,

[0102] ;

[0103] In the formula, α The meaning is the same as above, indicating the azimuth of the vertical surface of the arm; β 1 , β 2 , β 3 , β 4 They respectively represent the rotation angles of the 1st, 2nd, 3rd and 4th arm root joints (it may be stipulated that the initial state is 0 when it is coaxial with the upper arm frame).

[0104] Observation found that P 1 , P 2 , P 3 , P 4 , P 5 It's actually about α , β 1, β 2 , β 3 , β 4 Therefore, it can be simplified as:

[0105] . (1)

[0106] like Figure 4 As shown in the figure, there is usually an activity boundary of the boom around the construction machinery boom and on the ground (or other platform surface), which can be called an "electronic fence", which limits the construction machinery boom frame within the activity boundary. The boom activity boundary condition can be expressed as:

[0107] , (2)

[0108] , (3)

[0109] In the formula, Indicates the azimuth of the boom The pole distance of all parts on the boom, Indicates azimuth The pole distance of the active boundary, Indicates the height coordinates of all parts of the boom, Indicates the height coordinate of the ground (or other platform surface), △ ρ, △ z They respectively represent the reserved safety margins in the plane and height directions.

[0110] The robot arm can actively cross obstacles, which requires that the height of the arm frame at the plane where the obstacle is located is greater than the height of the obstacle. Therefore, the condition for the robot arm to actively avoid obstacles is expressed as:

[0111] ; (4)

[0112] like Figure 5 As shown in the figure, obstacles in engineering practice are mostly upright objects, which can be simplified as vertical cylinders. represents the height coordinate of the obstacle, Indicates the height coordinate of the boom frame at the plane where the obstacle is located, △ z Same meaning as above.

[0113] For long strip obstacles, they are divided into several obstacles, enveloping the original body, and the description of the obstacle can be set as a point set, that is:

[0114] {( α obsi , ρ obsi , zobsi , r obsi )}, i =1,2,…; (5)

[0115] In the formula, α obsi , ρ obsi , z obsi , r obsi They represent the obstacle's centroid azimuth, pole distance, vertex height, and envelope radius respectively.

[0116] The motion trajectory equations of the building mechanical arm can be expressed as:

[0117] , (6)

[0118] Or simply written as: , (7)

[0119] in, t is the time parameter, P 1 ( t ), P 2 ( t ), P 3 ( t ), P 4 ( t ), P 5 ( t ) satisfies formula (1), α , β 1 , β 2 , β 3 , β 4 Can be expressed as a parameter t Function:

[0120] , (8)

[0121] Formula (6) can also be rewritten as: , (9)

[0123] Formula (9) is the parameter t The equations describing the motion trajectory of the construction robot arm.

[0124] When the construction machinery boom moves, the end node of the fourth boom section moves from one point to another, that is, from the starting point to the end point. The starting point and the end point of the movement are respectively recorded as , , establish the start and end point constraints:

[0125] , (10)

[0126] , (11)

[0127] The path planning method for avoiding obstacles on a construction machine arm provided in this embodiment aims to achieve the effect that the starting point and the end point of the end node of the fourth arm section of the known arm are respectively recorded as , , find a set of equations { α ( t ), β 1 ( t ), β 2 ( t ), β 3 ( t ), β 4 ( t )}, under the conditions of satisfying equations (2)~(5), (9)~(11), the end of the robotic arm can complete the journey from the starting point to the destination in a certain target (such as the shortest time or the shortest time). To the end Active obstacle avoidance movement.

[0128] 2. Further describe step 2.

[0129] According to the design parameters of the building mechanical arm R 1 , R 2 , R 3 , R 4 , x 1 , z 1 Perform instantiation assignment.

[0130] According to the actual situation of the project site, the activity boundary (electronic fence) of the construction mechanical arm is instantiated and assigned:

[0131] ρ ex =ρ ex ( αex ), α ex ∈[0,2π],

[0132] z 0 = z ( α , ρ ),( α , ρ )∈Ω,

[0133] Wherein, Ω represents the movable area of ​​the frame.

[0134] According to the on-site obstacle location parameters, the obstacle parameters in the model are instantiated and assigned:

[0135] {( α obsi , ρ obsi , z obsi , r obsi )}, i =1,2,….

[0136] According to the construction tasks at the construction site, the starting point of the boom end is and end point Perform instantiation assignment:

[0137] .

[0138] Taking the construction of a concrete placing boom at a super high-rise building structure construction site as an example, according to the design parameters of the 4-arm “M” folding concrete placing boom, the lengths of the 1st, 2nd, 3rd, and 4th boom sections are assigned respectively. R 1 , R 2 , R 3 , R 4 , and the offset coordinates of the root node of the first boom section relative to the origin O (rotation center) in the boom elevation ( x 1 ,0, z 1); the activity boundary of the construction machinery boom is assigned according to parameters such as the fences on the four sides of the steel platform, the distance between the tower crane and the rotation center of the concrete placing boom, and the height of the construction platform; there are also obstacles such as structural rigid steel columns (plates), construction elevators, climbing steel columns, and steel bars extending out of the platform surface on site. The obstacles are assigned according to their centroid azimuth, polar distance, vertex height, and envelope radius; according to the construction tasks on the construction site, the spatial coordinates of the starting and ending points of the concrete placing boom boom are determined, and the starting and ending points of the boom end are assigned.

[0139] 3. Further describe step three.

[0140] In step 3, based on the instantiation data, we get { α ( t ), β 1 ( t ), β 2 ( t ), β 3 ( t ), β 4 ( t )}, satisfying the arm from the starting point To the end During the movement, the height of the boom frame at the plane where the obstacle is located is greater than the height of the obstacle, specifically:

[0141] Generate a training sample set for the machine learning algorithm based on instantiated data;

[0142] Use samples to train the machine learning algorithm, and use the trained algorithm function to solve the arm obstacle avoidance solution space;

[0143] The optimal path planning is performed for the end motion of the construction robot arm in the arm obstacle avoidance solution space.

[0144] Among them, the classification algorithm in the machine learning algorithm can be used, such as support vector machine, K nearest neighbor, decision tree, naive Bayes, random forest, neural network and other algorithms to perform sample set training.

[0145] For all α ∈[0,2π), β 1 ∈[0,π / 2), β 2 ∈(-π,0), β 3 ∈(0,π), β 4 ∈(-π,0), set a sufficiently large precision 1 / N 0 (N 0 is a positive integer), so that:

[0146] ,

[0147] ,

[0148] ,

[0149] ,

[0150] ,

[0151] in, i , j , k , l , m =0,1,2,…, N 0 .

[0152] Thus, a training sample set is established:

[0153] { α i , β 1j , β 2k , β 3l , β 4m , y ijklm}, i , j , k , l , m= 0,1,2,…, N 0 ,

[0154] The sample size is ( N 0 +1) 5 , 5 represents the number of degrees of freedom of the robot arm or the number of traversal subscript letters; the classification label of each sample y ijklm , determined according to the following rules:

[0155] , (12)

[0156] In formula (12), The situation indicates that the construction machinery arm frame is within the activity boundary and the arm frame actively avoids all obstacles; The situation indicates that the frame of the construction machinery arm is not within the activity boundary, or the frame of the arm has a position overlap conflict with the obstacle (that is, the obstacle is not avoided).

[0157] Among them, when using samples to train the machine learning algorithm to solve the arm obstacle avoidance solution space, the classification algorithm in the machine learning algorithm can be used, such as support vector machine, K nearest neighbor, decision tree, naive Bayes, random forest, neural network and other algorithms.

[0158] Taking the support vector machine (SVM) algorithm as an example (others are similar), for the above processed sample set, nonlinear kernel functions such as Gaussian kernel function and radial basis kernel function and appropriate parameters are selected to train the support vector machine (SVM) algorithm, and the training generates a support vector machine (SVM) algorithm function, which is recorded as:

[0159] y = F ( α , β 1 , β 2 , β 3 , β 4 ), (13)

[0160] Solve the obstacle avoidance solution space for all α ∈[0,2π), β 1 ∈[0,π / 2), β 2 ∈(-π,0), β 3 ∈(0,π), β 4 ∈(-π,0), such that y = F ( α , β 1 , β 2 , β 3 , β 4 )=1,( α , β 1 , β 2 , β 3 , β 4 ) can be within the range of the activity boundary and actively avoid all obstacles. That is, the obstacle avoidance solution space of the frame is:

[0161] . (14)

[0162] Specifically, in the obstacle avoidance solution space, the optimal path planning is performed for the end motion of the construction machinery arm. Specifically, in the obstacle avoidance solution space, the shortest motion path is taken as the goal (if the end motion speed is assumed to be constant, the shortest motion path is equivalent to the shortest time), and the dynamic programming algorithm is used to perform the optimal path planning for the end motion, such as Figure 6 As shown, the algorithm implementation steps are as follows:

[0163] A1. Set an optimal path point set first (The value of N is to be determined, it is an ordered set) to record the optimal path planned by the algorithm:

[0164] (15)

[0165] And there are:

[0166] (16)

[0167] A2. Set another starting point and end point Dynamic adjustment point between , The initial value is set to .

[0168] Dynamically update each Before, dynamically determine The coordinate position of Coordinate location basis To the end Whether it is determined by visual communication or not, the rules are as follows:

[0169] a. If To the end If the Dynamically adjust to ;

[0170] b. If To the end No line of sight, then, for the straight line The point set on the active boundary of the arm's active range , existence point P ex0 ( α ex0 , ρ ex0 , z 0 ) makes the angle , Dynamically adjust to P ex0 ( α ex0 , ρ ex0 , z 0 );

[0171] A3. Dynamic update under conditions .

[0172] Considering the influence of the fence, each moving step will Towards Specific direction Move one displacement control accuracy△ S arrive , and each mobile step arm is in the obstacle avoidance solution space within the fence activity space, expressed as:

[0173] , (17)

[0174] ; (18)

[0175] In the formula, express The unit vector of the direction, ;△ S The minimum moving distance for the end motion control of the boom;

[0176] α ( t n+1 ), β 1 ( t n+1 ), β 2 ( t n+1 ), β 3 ( t n+1 ), β 4 ( t n+1 ) satisfies formula (14), solves and records the updated input middle.

[0177] In order to facilitate practical engineering applications, a constant △ is introduced here d Indicates the accuracy of end motion control, that is, the unit moving step length (For example, it can be 0.01m), △ S andd The following relationship exists:

[0178] . (19)

[0179] A4. Set n=n+1, and repeat ②③ until .Right now:

[0180] a. When n=n+1, repeat the above processing steps ②③;

[0181] b. When n=n+1, N=n, , solve and record the update entry middle.

[0182] A5. Obtaining the equation system by fitting the function data The expression is:

[0183] According to the above solution results, it can be divided into:

[0184] .

[0185] Apply function fitting theory, such as using spline functions, to the point series Fit the functions separately , that is, we get the equation set (8).

[0186] The technical features of the above-described embodiments may be arbitrarily combined. To make the description concise, not all possible combinations of the technical features in the above-described embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0187] The above-mentioned embodiments only express several implementation modes of the present invention, and the description thereof is relatively specific and detailed, but it cannot be understood as limiting the scope of the invention. It should be pointed out that, for a person of ordinary skill in the art, several modifications and improvements can be made without departing from the concept of the present invention, and these all belong to the protection scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the attached claims.

Claims

1. A path planning method for obstacle avoidance of a construction robot arm, characterized in that: The steps include: Step 1: Establish a three-dimensional mathematical model of the motion of the construction mechanical arm, including establishing a set of motion trajectory equations of the construction mechanical arm; The three-dimensional cylindrical coordinate system is established with the arm support rotation center as the coordinate system origin O and the vertical upward direction as the positive direction of the z-axis. The azimuth angle of the arm is recorded as α ; The construction machine arm has M arm sections, and the lengths are R 1. R 2. … R M ; The angle between the first boom section and the horizontal plane is recorded as β 1, no. i Section Boom Section i -1 The angle between the arm sections is recorded as β i ,i=2,3,…,M;with time parameter t Establish the motion trajectory equations of the construction robot arm: ； In the formula, P 1 , P 2 , …, P M Respectively represent the root nodes of the 1st, 2nd, ..., Mth arm segments, P M+1 is the end node of the Mth arm segment, x 1, z 1 represents the offset coordinate of the root node of the first boom section relative to the origin O in the vertical plane of the boom; It also includes establishing that the construction machinery boom has an active boundary, and the boom active boundary condition is expressed as: ， ， In the formula, Indicates the azimuth of the boom α=α ex The pole distance of all parts on the boom, Indicates azimuth α=α ex The pole distance of the active boundary, Indicates the height coordinates of all parts of the boom, Z 0 represents the height coordinate of the ground or other platform surface, △ ρ, △ z Respectively represent the reserved safety margins in plane and height directions; Step 2: Assign values ​​to the mathematical model, determine the boom parameter M, and R 1. R 2. … R M The value of determines the obstacle point set and the starting point of the end node movement of the Mth arm section of the boom. and end point ; Step 3: Based on the instantiation data, get { α ( t ), β 1( t ), β 2( t ), β 3( t ), β 4( t )}, satisfying the arm from the starting point To the end During the movement, the height of the boom frame at the plane where the obstacle is located is greater than the height of the obstacle.

2. The path planning method for obstacle avoidance of a construction machine arm according to claim 1, characterized in that: In step 1, a three-dimensional mathematical model of the motion of the construction robot arm is established, which also includes establishing a mathematical expression for obstacles. If the obstacle is a vertical object, it is simplified as a vertical cylinder; if the obstacle is a long strip, it is divided into several obstacles that envelop the original object; The obstacle is described as a set of points, {( α obsi , ρ obsi , z obsi , r obsi )}, i =1,2,…; In the formula, α obsi , ρ obsi , z obsi , r obsi They represent the obstacle centroid azimuth, polar distance, vertex height, and envelope radius respectively; In step 2, the obstacle point set is determined, specifically: according to the actual situation of the construction site, the parameters are determined α obsi , ρ obsi , z obsi , r obsi .

3. The path planning method for obstacle avoidance of a construction machine arm as claimed in claim 2, characterized in that: In step 3, the height of the boom frame at the plane where the obstacle is located is greater than the height of the obstacle, which is expressed as: ； In the formula ρ is the distance from the point on the building robot arm to the origin O, △ z Indicates the safety margin reserved in the height direction. Pl represents the spatial position of the building robot arm, { Pl ( P 1 , P 2 ,…, P M , P M+1 )}, Pl i Represents a line segment P i P i+1 .

4. The path planning method for obstacle avoidance of a construction machine arm as claimed in claim 3, characterized in that: Step 2 also includes instantiating and assigning values ​​to the active boundaries of the construction robot arm: ρ ex =ρ ex ( α ex )， α ex ∈[0,2π], z 0= z ( α , ρ ),( α , ρ )∈Ω, Wherein, Ω represents the movable area of ​​the frame.

5. The path planning method for obstacle avoidance of a construction machine arm according to any one of claims 1 to 4, characterized in that: In step 3, based on the instantiation data, we get { α ( t ), β 1( t ), β 2( t ), β 3( t ), β 4( t )}, satisfying the arm from the starting point and end point During the movement, the height of the boom frame at the plane where the obstacle is located is greater than the height of the obstacle, specifically: Generate a training sample set for the machine learning algorithm based on instantiated data; Use samples to train the machine learning algorithm, and use the trained algorithm function to solve the arm obstacle avoidance solution space; The optimal path planning is performed for the end motion of the construction robot arm in the arm obstacle avoidance solution space.

6. The path planning method for obstacle avoidance of a construction machine arm as claimed in claim 5, characterized in that: M=4； In step 3, a training sample set for the machine learning algorithm is generated based on the instantiated data, including: For all α ∈[0,2π), β 1∈[0,π / 2), β 2∈(-π,0), β 3∈(0,π), β 4∈(-π,0), preset a precision of 1 / N 0, N 0 is a positive integer such that: ， ， ， ， ， in, i , j , k , l , m =0,1,2,…, N 0; Create a training sample set: { α i , β 1j , β 2k , β 3l , β 4m , y ijklm}， i , j , k , l , m= 0,1,2,…, N 0， The sample size is ( N 0+1) 5 The classification label of each sample y ijklm , determined according to the following rules: 。 7. The path planning method for obstacle avoidance of a construction machine arm as claimed in claim 6, characterized in that: In step 3, the machine learning algorithm is trained with samples, and the trained algorithm function is used to solve the arm obstacle avoidance solution space, including: The sample training machine learning algorithm adopts the support vector machine algorithm; For the established training sample set, a nonlinear kernel function and appropriate parameters are selected to train the support vector machine algorithm. The training generates a support vector machine algorithm function, which is recorded as: y = F ( α , β 1, β 2, β 3, β 4)， Solve the obstacle avoidance solution space for all α ∈[0,2π), β 1∈[0,π / 2), β 2∈(-π,0), β 3∈(0,π), β 4∈(-π,0), such that: y = F ( α , β 1, β 2, β 3, β 4)=1， ( α , β 1, β 2, β 3, β 4) The determined boom can be within the range of the activity boundary and actively avoid all obstacles, and the obstacle avoidance solution space of the boom is obtained as follows: 。 8. The path planning method for obstacle avoidance of a construction machine arm as claimed in claim 7, characterized in that: In step three, the optimal path planning is performed for the end motion of the construction machine arm in the arm obstacle avoidance solution space. Specifically, in the obstacle avoidance solution space, the shortest motion path is taken as the goal, and the dynamic programming algorithm is used to perform the optimal path planning for the end motion. The algorithm implementation steps are as follows: A1. Set an optimal path point set first , used to record the optimal path planned by the algorithm, where N The value of is to be determined, satisfying: ， And there are: ； A2. Set another starting point and end point Dynamic adjustment point between , The initial value is set to ; Dynamically update each Before, dynamically determine The coordinate position of Coordinate location basis To the end Whether it is determined by visual communication or not, the rules are as follows: a. If To the end If the Dynamically adjust to ; b. If To the end No line of sight, then, for the straight line - The point set on the active boundary of the arm's active range , existence point P ex0 ( α ex0 , ρ ex0 , z 0) Make the angle , Dynamically adjust to P ex0 ( α ex0 , ρ ex0 , z 0); A3. Dynamic update under conditions ; Considering the influence of the fence, each moving step will Towards Specific direction Move one displacement control accuracy△ S arrive , and each mobile step arm is in the obstacle avoidance solution space within the fence activity space, expressed as: ， ； In the formula, express The unit vector of the direction, ;△ S The minimum moving distance for the end motion control of the boom; ； △ d It represents the accuracy of the motion control of the end of the boom, which is a constant; Solve and record updates middle; A4. Order n=n+ 1, repeat A2 and A3 until ; Solve and record the update entry middle; A5. The equation set { α ( t ), β 1( t ), β 2( t ), β 3( t ), β 4( t )}.

Images