Method and system for planning operation path of stumping mechanical arm

Through the robotic arm operation path planning method, path pruning and Bezier curve optimization technology are used to solve the efficiency and quality problems of lemon strips on complex terrain, and efficient operations on slopes, bosses and pit terrains are achieved , avoid terrain barriers, improve operational stability and energy utilization efficiency.

CN119017387APending Publication Date: 2024-11-26HARBIN ENG UNIV

Patent Information

Application Number
CN202411236929.4
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-09-05
Publication Date
2024-11-26

AI Technical Summary

Technical Problem

The prior art is difficult to efficiently perform lemon strips on complex terrain, especially large mechanical equipment, which does not have the ability to adapt to the terrain, resulting in low operating efficiency, high labor intensity and difficulty in meeting agronomic requirements.

Method used

The robotic arm operation path planning method is adopted, and the time dimension calculation trajectory planning is introduced through path pruning and Bezier curve optimization, so as to realize the smooth operation of the robotic arm on complex terrain and continuous pose transformation.

Benefits of technology

This method can effectively plan the robotic arm operation path on slopes, bosses and pit terrains, avoid terrain obstacles, ensure good and smooth path quality, reduce energy consumption, and meet operation needs.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN119017387A_ABST
    Figure CN119017387A_ABST
Patent Text Reader

Abstract

The invention belongs to the technical field of mechanical arm path planning, and particularly relates to a stumping mechanical arm operation path planning method and system. The method comprises the steps that 1, path pruning and Bezier curve optimization are conducted on a stumping mechanical arm operation path, and a smooth stumping mechanical arm operation path is obtained; 2, time dimension calculation trajectory planning is introduced based on the stumping mechanical arm operation path smoothed in the step 1; and 3, realizing stable operation and completing continuous pose transformation based on the trajectory planning in the step 2. The stumping mechanism is used for solving the problems that mechanical arm paths are redundant and the stumping effect is poor.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention belongs to the technical field of robot arm path planning, and in particular relates to a method and system for planning an operation path of a stubble robot arm. Background Art

[0002] It is of great strategic significance to prevent and control land desertification and sustainably manage desertified land. Caragana korshinskii is an important windbreak and sand-fixing plant, but it will decline after 3-5 years of growth and needs to be regularly pruned and rejuvenated. At present, there are several ways to prune Caragana korshinskii in the Northwest region: one is to manually use tools to plan, chop, cut or use a brush cutter for pruning, but the operation efficiency is low and the labor intensity is high; the second is to use self-propelled machinery or large machinery similar to silage machines for pruning. This method does not have the ability to adapt to the terrain and is only suitable for flat terrain. The stubble height after pruning is inconsistent, the incision is not smooth, and it is difficult to meet the agronomic requirements of pruning. In the first case, there are problems such as high labor demand, difficulty in recruiting workers, and high labor costs. In the second case, large-scale silage equipment and shrub cutting equipment are only suitable for flat terrain, not suitable for the complex terrain of Caragana korshinskii pruning operation environment, and the pruning effect is not good.

[0003] Caragana is a multi-stem shrub with an inverted cone-shaped crown and sand bags at the roots. Caragana grows mostly in uneven sandy soil environments such as sandy slopes, arid hillsides, and sand dunes. The terrain where Caragana grows is generally continuous and smooth, but there are some raised terraces and pits. Generally, the artificial planting spacing is uniform. When stubble cutting, the stubble height should not be higher than 10cm to increase the sprouting rate of Caragana branches in the second year. Similarly, the stubble height of stubble cutting should not be too low, otherwise it will be covered by sand accumulation and affect the normal growth of Caragana. In addition, the incision of Caragana should be neat and smooth to avoid plant damage such as roughness and tearing. Summary of the invention

[0004] The invention provides a method for planning the operation path of a cropping robot arm, which solves the problems of redundant robot arm paths and poor cropping effect.

[0005] The invention provides a stubble robot arm operation path planning system, which is used to implement a stubble robot arm operation path planning method.

[0006] The present invention is achieved through the following technical solutions:

[0007] A method for planning a path for a stubble robot arm operation, the method comprising the following steps:

[0008] Step 1: Perform path pruning and Bezier curve optimization on the operation path of the stubble robot arm to obtain a smooth operation path of the stubble robot arm;

[0009] Step 2: Based on the smoothed operation path of the stubble robot arm in step 1, the time dimension is introduced to calculate the trajectory planning;

[0010] Step 3: Based on the trajectory planning in step 2, achieve smooth operation and complete continuous pose transformation.

[0011] Furthermore, the path pruning in step 1 is specifically as follows: firstly, inflection point detection is performed to analyze whether there are unnecessary inflection points that do not affect the target point on the line between each node and the adjacent point, and then the path is simplified to remove unnecessary inflection points and retain key inflection points of the path.

[0012] Furthermore, the Bezier curve optimization in step 1 is specifically to select key inflection points as control points in the path after pruning, and then generate the Bezier curve using the control points;

[0013] First, select the control points to connect, find points D and E on the connecting line and make:

[0014]

[0015] Connect DE and find point F according to the parabola three tangent theorem:

[0016]

[0017] Among them, A, B, C, are all selected control points; D, E and F are points found on the straight line to satisfy the formula;

[0018] Find all points that meet the conditions and remake the path curve. When given points P1, P2, P3…P n The Bezier curve formula is as follows:

[0019]

[0020] Expands to:

[0021]

[0022] The recursive expression is:

[0023]

[0024] Among them, B(t) is the real-time coordinate point of the Bezier curve, n is the order of the Bezier curve, i is the current order, 1, 2, 3, 4, 5, t is the time, and P is the given point.

[0025] Furthermore, the trajectory planning of step 2 includes joint space trajectory planning and Cartesian space trajectory planning;

[0026] The joint space trajectory planning specifically includes cubic polynomial interpolation and quintic polynomial interpolation;

[0027] The cubic polynomial interpolation is specifically:

[0028] Select the initial position and the end position to constrain the function. The initial position θ(t0) = θ0, the end position θ(t f )=θ f ;

[0029] The cubic polynomial interpolation form is:

[0030] θ(t)=a0+a1t+a2t 2 +a3t 3 (6)

[0031] Add constraints:

[0032]

[0033] Among them, a0...a3 are constant parameters of cubic polynomial three and quintic polynomial five. is the angular velocity at t0, t0 is the start time, t f For the end time, t f The angular velocity at

[0034] According to the constraint cubic polynomial equation, it is expressed as:

[0035]

[0036] The solution is:

[0037]

[0038] Robotics Toolbox is used to solve cubic polynomial interpolation equations and perform trajectory planning simulation.

[0039] Furthermore, the quintic polynomial interpolation is specifically:

[0040] θ(t)=a0+a1t+a2t 2 +a3t 3 +a4t 4 +a5t 5 (4-21)

[0041] The constraints are:

[0042]

[0043] It can be solved as:

[0044] a0=θ0 (11)

[0045]

[0046] in, is the angular velocity at t0, t f angular acceleration at , a5 is a constant parameter, a4 is a constant parameter.

[0047] Furthermore, the Cartesian space trajectory planning specifically includes linear interpolation and circular interpolation;

[0048] The linear interpolation is specifically to realize the linear trajectory planning from the starting point P1 (x1, y1, z1) to the target point P2 (x2, y2, z2) in Cartesian space;

[0049] The distance between P1(x1,y1,z1) and P2(x2,y2,z2) can be expressed as:

[0050]

[0051] The interpolation of N points is completed at equal intervals. The distance of a single interpolation segment is d, and the coordinates of the kth point P' moving in time t can be expressed as

[0052] Shown as:

[0053]

[0054] Where L is the distance from P1 to P2,

[0055] Assume λ = i / d, the maximum uniform speed is V max , acceleration is a, acceleration time is T m , acceleration section displacement L m ,

[0056] T m , L m It can be expressed as:

[0057]

[0058] Assume the acceleration time is T b , uniform time T a It can be expressed as:

[0059]

[0060] The total movement time T can be expressed as:

[0061]

[0062] Normalize the displacement, velocity, and acceleration of the parabola fitting segment: L mλ =L m / T,T mλ =T m / T,a λ =2L mλ / T mλ2 , we can get:

[0063]

[0064] When λ=0, it is the starting point, and λ=1, it is the target point, so the position of each interpolation point can be calculated.

[0065] Furthermore, the arc interpolation is specifically that three non-collinear points P1 (x1, y1, z1), P2 (x2, y2, z2), and P3 (x3, y3, z3) in Cartesian space can determine a circle, and the generated arc is converted to a plane, and then converted into Cartesian space interpolation coordinates after the interpolation is completed;

[0066] The plane composed of P1(x1,y1,z1), P2(x2,y2,z2), and P3(x3,y3,z3) can be expressed as:

[0067]

[0068] Using A1x+B1y+C1z+D1=0 to represent the plane, we can get the coefficients:

[0069]

[0070] The perpendicular plane S1 of P1(x1,y1,z1)P2(x2,y2,z2) can be expressed as:

[0071]

[0072] Using A2x+B2y+C2z+D2=0 to represent the S1 plane, we can get the coefficients:

[0073]

[0074] Similarly, A3x+B3y+C3z+D3=0 can be used to express the perpendicular plane S2 of P2(x2,y2,z2)P3(x3,y3,z3) as:

[0075]

[0076] By combining the plane equations, we can get:

[0077]

[0078] Assume the coordinates of the arc center are Q(x0,y0,z0), the coordinates can be expressed as:

[0079]

[0080] The radius size is:

[0081]

[0082] With QP1 as the u-axis and the arc normal vector as the w-axis, the v-axis is created by the right-hand rule to establish the coordinate system. The u-axis unit vector can be expressed as:

[0083]

[0084] The w-axis unit vector is obtained:

[0085]

[0086] in:

[0087]

[0088] The transformation matrix of the original coordinate system to the coordinate system with Q as the base point can be expressed as:

[0089]

[0090] Assume the interpolation point P' = (u, v, w), where the center angle is ω, and the center angle of the arc connecting P1 (x1, y1, z1) and P3 (x3, y3, z3) is θ, then the number of interpolation can be expressed as:

[0091]

[0092] Then the coordinates of the i-th interpolation point can be expressed as:

[0093]

[0094] The transformation matrix converted to the original coordinate system can be expressed as:

[0095]

[0096] According to inverse kinematics, the inverse solution of each interpolation point is obtained to complete the Cartesian space arc interpolation trajectory planning.

[0097] A stubble robot arm operation path planning system, the system uses the stubble robot arm operation path planning method as described above, the system comprises:

[0098] The stubble robot arm operation path optimization module performs path pruning and Bezier curve optimization on the stubble robot arm operation path to obtain a smooth stubble robot arm operation path;

[0099] The trajectory planning module of the stubble robot arm is used to introduce the time dimension to calculate the trajectory planning based on the smooth operation path of the stubble robot arm; to achieve smooth operation and complete continuous posture transformation.

[0100] A computer device comprises a memory, a processor and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, the above method is implemented.

[0101] A computer-readable storage medium stores a computer program, and the computer program implements the above method when executed by a processor.

[0102] The beneficial effects of the present invention are:

[0103] The present invention can plan an effective operation path for the robot arm along the terrain and avoid the terrain and inoperable obstacles. The robot arm path has good quality and is smooth.

[0104] The improved algorithm of the present invention can not only plan an effective motion path but also ensure the motion stability of the robot arm and reduce energy consumption.

[0105] The present invention can complete the work of the stubble leveling mechanical arm on slope terrain, convex platform terrain and concave pit terrain.

[0106] The stubble-leveling robot arm of the present invention does not collide with terrain obstacles, thus meeting the operation requirements. BRIEF DESCRIPTION OF THE DRAWINGS

[0107] Figure 1 It is a schematic diagram of the simulation results of the RRT and RRT* obstacle avoidance path planning of the present invention.

[0108] Figure 2 It is a schematic diagram of the simulation results of the path pruning and Bezier curve optimization obstacle avoidance path planning of the present invention.

[0109] Figure 3 It is a flow chart of the joint space trajectory planning program of the present invention.

[0110] Figure 4 It is a schematic diagram of the simulation of the cubic polynomial trajectory planning of a single-segment path of the present invention, wherein (a) is a schematic diagram of the simulation result of the moving joint, and (b) is a schematic diagram of the simulation result of the rotating joint.

[0111] Figure 5 It is a schematic diagram of the simulation of the multi-segment path cubic polynomial trajectory planning of the present invention, wherein (a) is a schematic diagram of the simulation result of the moving joint, and (b) is a schematic diagram of the simulation result of the rotating joint.

[0112] Figure 6 It is a schematic diagram of the simulation results of the multi-segment path quintic polynomial trajectory planning of the present invention, wherein (a) is a schematic diagram of the simulation results of the moving joint, and (b) is a schematic diagram of the simulation results of the rotating joint.

[0113] Figure 7 It is a schematic diagram of the simulation results of the linear interpolation trajectory planning of the present invention.

[0114] Figure 8 It is a schematic diagram of the simulation results of the arc interpolation trajectory planning of the present invention.

[0115] Fig. 9 Schematic diagram of complex terrain for the stubble robot arm of the present invention, wherein (a) is a schematic diagram of a slope terrain, (b) is a schematic diagram of a pit terrain, and (c) is a schematic diagram of a boss terrain.

[0116] Fig.10 It is a simulation flow chart of the present invention.

[0117] Fig.11 It is a schematic diagram of the simulation of the obstacle avoidance path planning for complex terrain of the present invention, wherein (a) is a schematic diagram of the simulation results of slope terrain, (b) is a schematic diagram of the simulation results of pit terrain (front view), (c) is a schematic diagram of the simulation results of pit terrain (top view), (d) is a schematic diagram of the simulation results of platform terrain (front view), and (e) is a schematic diagram of the simulation results of platform terrain (top view).

[0118] Fig.12 It is a schematic diagram of the slope operation simulation process of the present invention, wherein (a) is a schematic diagram of slope operation posture 1, (b) is a schematic diagram of slope operation posture 2, and (c) is a schematic diagram of slope operation posture 3.

[0119] Fig.13 It is a schematic diagram of the torque curves of the joints of the slope working robot arm of the present invention, wherein (a) is a schematic diagram of the horizontal moving arm torque, (b) is a schematic diagram of the rotary joint torque, (c) is a schematic diagram of the lifting arm torque, (d) is a schematic diagram of the rotation arm torque, and (e) is a schematic diagram of the swing arm torque.

[0120] Fig.14 It is a schematic diagram of the boss operation simulation process of the present invention, wherein (a) is a schematic diagram of boss operation posture 1, (b) is a schematic diagram of boss operation posture 2, and (c) is a schematic diagram of boss operation posture 3.

[0121] Fig.15 It is a schematic diagram of the torque curves of the joints of the boss operation robot arm of the present invention, wherein (a) is a schematic diagram of the horizontal movement arm torque, (b) is a schematic diagram of the rotation joint torque, (c) is a schematic diagram of the lifting arm torque, (d) is a schematic diagram of the rotation arm torque, and (e) is a schematic diagram of the swing arm torque.

[0122] Fig.16 It is a schematic diagram of the pit operation simulation process of the present invention, wherein (a) is a schematic diagram of pit operation posture 1, (b) is a schematic diagram of pit operation posture 2, and (c) is a schematic diagram of pit operation posture 3.

[0123] Fig.17It is a schematic diagram of the torque curves of the joints of the pit operation robot arm of the present invention, wherein (a) is a schematic diagram of the horizontal movement arm torque, (b) is a schematic diagram of the rotation joint torque, (c) is a schematic diagram of the lifting arm torque, (d) is a schematic diagram of the rotation arm torque, and (e) is a schematic diagram of the swing arm torque.

[0124] Fig.18 It is a flow chart of the method of the present invention. DETAILED DESCRIPTION

[0125] In the following description, specific details such as specific system structures, technologies, etc. are provided for the purpose of illustration rather than limitation, so as to provide a thorough understanding of the embodiments of the present application. However, it should be clear to those skilled in the art that the present application may also be implemented in other embodiments without these specific details. In other cases, detailed descriptions of well-known systems, devices, circuits, and methods are omitted to prevent unnecessary details from obstructing the description of the present application.

[0126] It should be understood that when used in this specification and the appended claims, the term "comprising" indicates the presence of described features, integers, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components and / or groups thereof.

[0127] It should also be understood that the terms used in this application specification are only for the purpose of describing specific embodiments and are not intended to limit the application. As used in this application specification and the appended claims, the singular forms "a", "an" and "the" are intended to include plural forms unless the context clearly indicates otherwise.

[0128] The following is attached to this application specification Figure 1-16 , the technical solutions in the embodiments of the present application are clearly and completely described. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present application.

[0129] In the following description, many specific details are set forth to facilitate a full understanding of the present application, but the present application may also be implemented in other ways different from those described herein, and those skilled in the art may make similar generalizations without violating the connotation of the present application. Therefore, the present application is not limited to the specific embodiments disclosed below.

[0130] A method for planning a path for a stubble robot arm operation, the method comprising the following steps:

[0131] Step 1: Perform path pruning and Bezier curve optimization on the operation path of the stubble robot arm to obtain a smooth operation path of the stubble robot arm;

[0132] Step 2: Based on the smoothed operation path of the stubble robot arm in step 1, the time dimension is introduced to calculate the trajectory planning;

[0133] Step 3: Based on the trajectory planning in step 2, achieve smooth operation and complete continuous pose transformation.

[0134] Furthermore, the basic RRT algorithm is that the RRT algorithm takes the starting point as the root node, expands the leaf nodes by random sampling, and repeats this process to form a random tree structure. When a leaf node reaches the target point, the target point begins to trace back to its parent node until it returns to the starting point, thereby constructing a path from the starting point to the target point.

[0135] In the figure: q init is the starting point; q nearest is the nearest node; q new is a new node; q rand is a randomly generated node; q goal is the target point; η is the step size.

[0136] The RRT* algorithm first calculates the new node q based on RRT. new The cost to the node within range r, search for the minimum cost node q min Replace the original parent node q nearest , connecting node q min With q new , recalculate q new Node cost, after completing q new After the node cost is calculated, rewiring is performed. If the obstacle detection is passed, the nodes within the range r are calculated with q new is the new cost of the parent node. If the new cost is lower than the original cost, the updated cost and the new parent node are q new , q new Add to the tree to check whether the target point has been reached. If the target point has been reached, the path will be generated in reverse order. Otherwise, q will continue to be generated. rand Complete the loop.

[0137] Set up complex obstacles for obstacle avoidance simulation and compare the path planning effects of RRT and RRT*. The simulation results are as follows: Figure 1 It can be seen that RRT* has a stronger search purpose, increases the goal orientation and reduces the blindness caused by random sampling. The RRT simulation path length is 1874, and the RRT* simulation path length is 1802. It can be seen that RRT* reduces the path cost, converges faster and has better path quality, but the path is still relatively redundant, not concise and direct, and the smoothness needs to be improved.

[0138] The path pruning in step 1 is specifically to perform path pruning to solve the path redundancy problem. First, inflection point detection is performed to analyze whether there are unnecessary inflection points that do not affect the target point on the line between each node and the adjacent point. Then, path simplification is completed to remove unnecessary inflection points and retain key inflection points of the path. Path pruning can also reduce movement time.

[0139] Furthermore, the Bezier curve optimization in step 1 is specifically to select key turning points as control points in the path after pruning to improve the smoothness of the operation path, and then use the control points to generate Bezier curves. Bezier curve optimization can also improve path accuracy, reduce operation energy consumption, and reduce the number of starts and stops of the stubble robot arm, thereby improving operation stability and safety.

[0140] First, select the control points to connect, find points D and E on the connecting line and make:

[0141]

[0142] Connect DE and find point F according to the parabola three tangent theorem:

[0143]

[0144] Among them, A, B, C, are all selected control points; D, E and F are points found on the straight line to satisfy the formula;

[0145] Find all points that meet the conditions and remake the path curve. When given points P1, P2, P3…P n The Bezier curve formula is as follows:

[0146]

[0147] Expands to:

[0148]

[0149] The recursive expression is:

[0150]

[0151] Among them, B(t) is the real-time coordinate point of the Bezier curve, n is the order of the Bezier curve, i is the current order, 1, 2, 3, 4, 5, t is the time, and P is the given point.

[0152] Based on RRT*, pruning and Bezier curve optimization are introduced to perform path planning simulation. The results are as follows Figure 2 .

[0153] After path pruning and Bezier curve optimization, the simulation path lengths are 1756 and 1576 respectively, which significantly reduces path redundancy and effectively improves path smoothness, reduces the number of robot arm starts and stops, and increases overall stability and safety.

[0154] Furthermore, in order to fully consider the dynamic characteristics of the robotic arm, time dimension calculation, namely trajectory planning, is introduced on the basis of path planning. The robotic arm can be planned to complete complex combined movements under physical constraints such as speed and acceleration, and can effectively improve the efficiency and accuracy of the robotic arm. Trajectory planning can be established in Cartesian space and joint space. Trajectory planning in Cartesian space is more intuitive, but the amount of solution calculation is large and it is easy to encounter mechanism singularities. Trajectory planning in joint space does not require an inverse solution to obtain joint variables, but requires control of control accuracy.

[0155] The trajectory planning of step 2 includes joint space trajectory planning and Cartesian space trajectory planning; the joint space trajectory planning is to combine the change of joint variable parameters with time variables to form a function, and the initial position and terminal position information of the flattening robot arm need to be given to fit the joint parameter change value, and the motion trajectory is expressed as a function curve of angle, angular velocity and acceleration; the curve can be fitted according to different interpolation methods;

[0156] The joint space trajectory planning specifically includes cubic polynomial interpolation and quintic polynomial interpolation;

[0157] The cubic polynomial interpolation is specifically:

[0158] Select the initial position and the end position to constrain the function. The initial position θ(t0) = θ0, the end position θ(t f )=θ f ;

[0159] The cubic polynomial interpolation form is:

[0160] θ(t)=a0+a1t+a2t 2 +a3t 3 (6)

[0161] Add constraints:

[0162]

[0163] Among them, a0...a3 are constant parameters of cubic polynomial three and quintic polynomial five. is the angular velocity at t0, t0 is the start time, t f For the end time, t f The angular velocity at

[0164] According to the constraint cubic polynomial equation, it is expressed as:

[0165]

[0166] The solution is:

[0167]

[0168] Robotics Toolbox is used to solve the cubic polynomial interpolation equation and perform trajectory planning simulation. The process is as follows: Figure 3 .

[0169] Perform two-point single-segment path simulation, input parameter variables of different joint positions as shown in Table 1, set the sampling time to 4s, the interval to 0.01s, and convert the moving joint angle according to the lead 16. The simulation results are shown in Figure 4 . Figure 4 It can be seen that the angle, speed, and acceleration curves of each joint motion in a single-segment path are relatively smooth, meeting the operational requirements.

[0170] Table 1 Input values ​​of single-segment path simulation parameter variables

[0171]

[0172] In order to detect the dynamic performance of the robot arm during continuous operation, a multi-segment path simulation was performed. The parameter variables of different joint positions were input as shown in Table 2. The sampling time was set to 8s and the interval was 0.01s. The simulation results are shown in Figure 5 . Figure 5 When the cubic polynomial trajectory is planned, there is a sudden change in acceleration at the intersection of the path, that is, there is a sudden change point where the slope of the acceleration curve changes suddenly, which will cause the robot arm movement impact and reduce the overall stability.

[0173] Table 2 Input values ​​of multi-segment path simulation parameter variables

[0174]

[0175] Furthermore, the quintic polynomial interpolation is specifically to introduce quintic polynomial interpolation for trajectory planning in order to reduce acceleration mutation and ensure trajectory smoothness;

[0176] The fifth-degree polynomial is in the form:

[0177] θ(t)=a0+a1t+a2t 2 +a3t 3 +a4t 4 +a5t 5 (4-21)

[0178] The constraints are:

[0179]

[0180] It can be solved as:

[0181] a0=θ0 (11)

[0182]

[0183] in, is the angular velocity at t0, t f Angular acceleration at time, a5 is a constant parameter, a4 is a constant parameter,

[0184] Input parameter variables of different joint positions as shown in Table 2, the sampling time is set to 8s, the interval is 0.01s, and the simulation results of multi-segment path quintic polynomial are shown in Figure 6 .

[0185] Figure 6 It can be seen that the angle and angular velocity interpolation fitting effect of the quintic polynomial trajectory planning is better, the slope of the curve changes more gently in each stage, and the mutation points of the sudden change of the slope of the angular acceleration curve are effectively reduced, and the smoothness of the curve is improved, which means that the acceleration mutation phenomenon during the movement of the stubble robot arm is significantly improved, which can greatly increase the stability and safety of the operation and meet the operation requirements.

[0186] Furthermore, the Cartesian space trajectory planning specifically includes linear interpolation and circular interpolation;

[0187] The linear interpolation is specifically to realize the linear trajectory planning from the starting point P1 (x1, y1, z1) to the target point P2 (x2, y2, z2) in Cartesian space;

[0188] The distance between P1(x1,y1,z1) and P2(x2,y2,z2) can be expressed as:

[0189]

[0190] The interpolation of N points is completed at equal intervals. The coordinates of the kth point P' moved to in time t after a single interpolation segment distance d can be expressed as:

[0191]

[0192] Where L is the distance from P1 to P2,

[0193] In order to smooth the start-stop acceleration, a parabola is added to complete the fitting, assuming λ = i / d, and the maximum uniform speed is V max , acceleration is a, acceleration time is T m , acceleration section displacement L m , T m , L m It can be expressed as:

[0194]

[0195] Assume the acceleration time is T b , uniform time T a It can be expressed as:

[0196]

[0197] The total movement time T can be expressed as:

[0198]

[0199] Normalize the displacement, velocity, and acceleration of the parabola fitting segment: L mλ =L m / T,T mλ =T m / T,a λ =2L mλ / T mλ 2 , we can get:

[0200]

[0201] When λ=0 is the starting point and λ=1 is the target point, the position of each interpolation point can be calculated. The starting point is set to P1(0,-300,100)P1(0,-300,100) and the target point is set to P2(220,410,900) for simulation. The simulation results are as follows: Figure 7 .

[0202] Figure 7 The obtained trajectory is a straight line passing through the set starting point and target point, which can verify the correctness of the linear interpolation derivation, and the simulated straight line trajectory is continuous and uninterrupted, indicating that when the flattening robot arm performs motion operations according to the linear interpolation trajectory planning, it can run smoothly as a whole and complete continuous posture changes, which meets the operation requirements.

[0203] Furthermore, the arc interpolation is specifically that three non-collinear points P1 (x1, y1, z1), P2 (x2, y2, z2), and P3 (x3, y3, z3) in Cartesian space can determine a circle, and the generated arc is converted to a plane, and then converted into Cartesian space interpolation coordinates after the interpolation is completed;

[0204] The plane composed of P1(x1,y1,z1), P2(x2,y2,z2), and P3(x3,y3,z3) can be expressed as:

[0205]

[0206] Using A1x+B1y+C1z+D1=0 to represent the plane, we can get the coefficients:

[0207]

[0208] The perpendicular plane S1 of P1(x1,y1,z1)P2(x2,y2,z2) can be expressed as:

[0209]

[0210] Using A2x+B2y+C2z+D2=0 to represent the S1 plane, we can get the coefficients:

[0211]

[0212] Similarly, A3x+B3y+C3z+D3=0 can be used to express the perpendicular plane S2 of P2(x2,y2,z2)P3(x3,y3,z3) as:

[0213]

[0214] By combining the plane equations, we can get:

[0215]

[0216] Assume the coordinates of the arc center are Q(x0,y0,z0), the coordinates can be expressed as:

[0217]

[0218] The radius size is:

[0219]

[0220] With QP1 as the u-axis and the arc normal vector as the w-axis, the v-axis is created by the right-hand rule to establish the coordinate system. The u-axis unit vector can be expressed as:

[0221]

[0222] The w-axis unit vector is obtained:

[0223]

[0224] in:

[0225]

[0226] The transformation matrix of the original coordinate system to the coordinate system with Q as the base point can be expressed as:

[0227]

[0228] Assume the interpolation point P' = (u, v, w), where the center angle is ω, and the center angle of the arc connecting P1 (x1, y1, z1) and P3 (x3, y3, z3) is θ, then the number of interpolation can be expressed as:

[0229]

[0230] Then the coordinates of the i-th interpolation point can be expressed as:

[0231]

[0232] The transformation matrix converted to the original coordinate system can be expressed as:

[0233]

[0234] According to inverse kinematics, the inverse solution of each interpolation point is obtained to complete the Cartesian space arc interpolation trajectory planning. Set P1 (184,248,794), P2 (146,475,832), and P3 (169,654,809) for simulation. The simulation results are as follows: Figure 8 .

[0235] Figure 8 The obtained trajectory is an arc curve passing through the set starting point, intermediate point and target point, which can verify the correctness of the arc interpolation derivation, and the simulated arc trajectory is continuous and smooth without mutations, indicating that when the flattening robot arm performs motion operations according to the circular interpolation trajectory planning, it can run smoothly as a whole and complete continuous posture changes, which meets the operation requirements.

[0236] Specifically, the working environment of the stubble robot arm is as follows Fig. 9 To avoid collision with obstacles such as terrain, slopes, pits, and convex platforms, an improved algorithm is used to complete obstacle avoidance path planning simulation on the simulated terrain. Appropriate obstacles are added to simulate obstacles such as non-operable plants, and simulation is completed on slopes, pits, and convex platforms. The simulation process is as follows: Fig.10 .

[0237] The simulation results are as follows Fig.11 It can be found that in complex environments, the improved algorithm can plan an effective working path along the terrain and avoid terrain and inoperable obstacles. The path quality is good and smooth, which meets the path planning requirements of the stubble robot arm. The verification algorithm can not only plan an effective motion path but also ensure the stability of the robot arm motion and reduce energy consumption.

[0238] The specific simulation of robot arm motion and operation is as follows:

[0239] A. Slope terrain operation simulation

[0240] Set the slope terrain, add feed resistance at the cutter, make the cutter work from the top of the slope to the bottom of the slope, and constantly adjust the cutter angle to complete the simulation. The simulation process is as follows: Fig.12 .

[0241] During the movement, the horizontal moving arm continues to move forward to adjust the front and rear position of the cutter on the slope. The slewing joint turns left to move the cutter from the right to the left. The lifting arm and the rotating arm control the overall up and down position. From position 1 to position 3, the swing arm continuously adjusts the angle of the cutter to make the cutter fit the terrain. The overall movement process is smooth and does not collide with terrain obstacles, meeting the operation requirements. The torque changes of each joint are as follows: Fig.13 .

[0242] Depend on Fig.13 The operation time is 0-6s. During the movement, the torque of each joint changes continuously and smoothly, and the horizontal moving arm and the lifting arm are not affected by the feed resistance in the movement direction. The torque value changes slightly, which is in line with the expected setting. The maximum torque of each joint is less than the allowable torque of the selected motor, which meets the operation requirements.

[0243] B. Boss Terrain Operation Simulation

[0244] Set the boss terrain, add feed resistance at the cutter, and make the cutter move from the bottom of the boss along the terrain to the top of the boss. After working horizontally at the top for a period of time, move to the bottom of the boss on the other side, and continuously adjust the cutter angle to complete the simulation. The simulation process is as follows: Fig.14 .

[0245] The horizontal moving arm adjusts the front and rear positions, and the slewing joint turns left to make the cutter move from the right to the left. The lifting arm first rises to the maximum amplitude, and then the swing arm rises and stays at the top of the slope for a while. After crossing the boss obstacle, the swing arm drops to its original position, and then the lifting arm drops to make the cutter reach the bottom of the boss on the other side. From position 1 to position 3, the swing arm continuously adjusts the angle of the cutter to make the cutter fit the terrain. The overall movement process is smooth and does not collide with the terrain obstacles, meeting the operation requirements. The torque changes of each joint are as follows: Fig.15 .

[0246] Depend on Fig.15 During the operation time of 0-6s, the torque curves of each joint are smooth as a whole. The cutting knife stays in the horizontal direction at the top of the boss for 2.5s-3.5s. Except for the rotary joint, the torque of each joint stops changing. The cutting knife continues to move in the horizontal direction for 3.5s-6s, and the torque of each joint continues to change. The horizontal moving arm and the lifting arm are not affected by the feed resistance in the direction of movement, and the torque value changes slightly. The maximum torque of each joint is less than the allowable torque of the selected motor to meet the operation requirements.

[0247] C. Simulation of pit terrain operation

[0248] Set the pit terrain, add feed resistance at the cutter, and make the cutter move from the top edge of the pit along the terrain to the bottom of the pit. After working horizontally at the bottom for a period of time, move to the top of the pit on the other side, and continuously adjust the cutter angle to complete the simulation. The simulation process is as follows: Fig.16 .

[0249] The horizontal moving arm adjusts the front and rear working positions of the cutting knife. The slewing joint turns left to control the cutting knife to move from the right edge to the left edge. The lifting arm first drops to the maximum amplitude, and then the swing arm drops, and keeps working at the bottom of the pit for a while. After crossing the bottom of the pit, the swing arm rises to the original position, and then the lifting arm rises to make the cutting knife reach the top edge of the pit on the other side. From position 1 to position 3, the swing arm continuously adjusts the cutting knife angle to make the cutting knife fit the terrain. The overall movement process is smooth and does not collide with terrain obstacles, meeting the working requirements. The torque changes of each joint are as follows: Fig.17 .

[0250] Depend on Fig.17 The operation time is 0-6s. The torque curves of each joint are smooth as a whole during the movement. At the bottom of the pit, the cutting knife maintains a horizontal direction for 2s-4s. Except for the rotating arm, the joint torque remains unchanged for 4s-6s. The cutting knife resumes horizontal movement. The torque of each joint changes. The horizontal moving arm and the lifting arm are not affected by the feed resistance in the direction of movement. The torque value changes slightly, which is in line with the expected setting. The maximum torque of each joint is less than the allowable torque of the selected motor, which meets the operation requirements.

[0251] The robotic arm completed simulation experiments on slope, boss and pit terrain operations. The simulation results showed that the robotic arm was able to complete the specified actions and the power met the operating requirements.

[0252] Implementation Method 2

[0253] This embodiment provides a stubble robot arm operation path planning system, the system uses the stubble robot arm operation path planning method as described in the first embodiment, the system includes:

[0254] The stubble robot arm operation path optimization module performs path pruning and Bezier curve optimization on the stubble robot arm operation path to obtain a smooth stubble robot arm operation path;

[0255] The trajectory planning module of the stubble robot arm is used to introduce the time dimension to calculate the trajectory planning based on the smooth operation path of the stubble robot arm; to achieve smooth operation and complete continuous posture transformation.

[0256] Implementation Method 3

[0257] An embodiment of the present invention provides an electronic device, which includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the memory is used to store software programs and modules, and the processor executes various functional applications and data processing by running the software programs and modules stored in the memory. The memory and the processor are connected via a bus. Specifically, the processor implements any step in the first embodiment above by running the computer program stored in the memory.

[0258] It should be understood that in the embodiments of the present invention, the processor referred to may be a central processing unit (CPU), and the processor may also be other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or the processor may also be any conventional processor, etc.

[0259] The memory may include a read-only memory, a flash memory, and a random access memory, and provides instructions and data to the processor. A part or all of the memory may also include a nonvolatile random access memory.

[0260] It can be seen from the above that the electronic device provided in the embodiment of the present invention can implement the stubble robot arm operation path planning method as described in embodiment one by running a computer program. The present invention can plan an effective operation path for the robot arm along the terrain, and avoid the terrain and inoperable obstacles. The robot arm path quality is good and smooth.

[0261] It should be understood that if the above-mentioned integrated module / unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on such an understanding, the present invention implements all or part of the processes in the above-mentioned implementation method, and can also be completed by instructing the relevant hardware through a computer program. The above-mentioned computer program can be stored in a computer-readable storage medium, and the computer program can implement the steps of the above-mentioned various method implementations when executed by the processor. Among them, the above-mentioned computer program includes computer program code, and the above-mentioned computer program code can be in source code form, object code form, executable file or some intermediate form. The above-mentioned computer-readable medium may include: any entity or device capable of carrying the above-mentioned computer program code, recording medium, U disk, mobile hard disk, disk, optical disk, computer memory, read-only memory (ROM), random access memory (RAM), electric carrier signal, telecommunication signal and software distribution medium. It should be noted that the content contained in the above-mentioned computer-readable storage medium can be appropriately increased or decreased according to the requirements of legislation and patent practice in the jurisdiction.

[0262] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest range consistent with the principles and novel features disclosed herein.

[0263] Those skilled in the art can clearly understand that for the convenience and simplicity of description, only the division of the above-mentioned functional units and modules is used as an example for illustration. In actual applications, the above-mentioned function allocation can be completed by different functional units and modules as needed, that is, the internal structure of the above-mentioned device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the implementation method can be integrated into a processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The above-mentioned integrated unit can be implemented in the form of hardware or in the form of software functional units. In addition, the specific names of the functional units and modules are only for the convenience of distinguishing each other, and are not used to limit the scope of protection of the present invention. The specific working process of the units and modules in the above-mentioned system can refer to the corresponding process in the aforementioned method implementation method, which will not be repeated here.

[0264] It should be noted that the methods and detailed examples provided in the above embodiments can be combined with the devices and equipment provided in the embodiments, and references can be made to each other, and no further details will be given.

[0265] Those skilled in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of the present invention.

[0266] In the embodiments provided by the present invention, it should be understood that the disclosed device / terminal equipment and method can be implemented in other ways. For example, the device / equipment implementation described above is only illustrative, for example, the division of the above modules or units is only a logical function division, and in actual implementation, there may be other division methods, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed.

[0267] The above-mentioned embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit them. Although the present invention has been described in detail with reference to the above-mentioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the above-mentioned embodiments, or replace some of the technical features therein by equivalents. These modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the various embodiments of the present invention, and should all be included in the protection scope of the present invention.

Claims

1. A method for planning the operation path of a stubble robot arm. It is characterized in that The method comprises the following steps, Step 1: Perform path pruning and Bezier curve optimization on the operation path of the stubble robot arm to obtain a smooth operation path of the stubble robot arm; Step 2: Based on the smoothed operation path of the stubble robot arm in step 1, the time dimension is introduced to calculate the trajectory planning; Step 3: Based on the trajectory planning in step 2, achieve smooth operation and complete continuous pose transformation.

2. According to the method for planning the operation path of a stubble robot arm according to claim 1, It is characterized in that Specifically, the path pruning in step 1 is to first perform inflection point detection, analyze whether there are unnecessary inflection points that do not affect the target point on the line between each node and the adjacent point, and then complete the path simplification, remove unnecessary inflection points and retain the key inflection points of the path.

3. According to the method for planning the operation path of a stubble robot arm as described in claim 2, It is characterized in that The Bezier curve optimization in step 1 is specifically to select key inflection points as control points in the path after pruning, and then generate the Bezier curve using the control points; Select the control points to connect, find points D and E on the connecting line and make: Connect DE and find point F according to the parabola three tangent theorem: Among them, A, B, C, are all selected control points; D, E and F are points found on the straight line to satisfy the formula; Find all points that meet the conditions to reconstruct the path curve. When a given point P 1 , P 2 , P 3 …P n The Bezier curve formula is as follows: Expands to: The recursive expression is: P 0 n =(1-t)P 0 n-1 +tP 1 n-1 ,t∈[0,1] (5) Among them, B(t) is the real-time coordinate point of the Bezier curve, n is the order of the Bezier curve, i is the current order, 1, 2, 3, 4, 5, t is the time, and P is the given point.

4. According to the method for planning the operation path of a stubble robot arm as described in claim 1, It is characterized in that The trajectory planning of step 2 includes joint space trajectory planning and Cartesian space trajectory planning; The joint space trajectory planning specifically includes cubic polynomial interpolation and quintic polynomial interpolation; The cubic polynomial interpolation is specifically: Select the initial position and the end position to constrain the function. The initial position θ(t 0 )=θ 0 , end point position θ(t f )=θ f ; The cubic polynomial interpolation form is: θ(t)=a 0 +a 1 t+a 2 t 2 +a 3 t 3 (6) Add constraints: Among them, a 0 ...a 3 For constant parameters cubic polynomial three quintic polynomial five, t 0 Angular velocity at time, t 0 is the start time, t f For the end time, t f The angular velocity at According to the constraint cubic polynomial equation, it is expressed as: The solution is: Robotics Toolbox is used to solve cubic polynomial interpolation equations and perform trajectory planning simulation.

5. According to the method for planning the operation path of a stubble robot arm according to claim 4, It is characterized in that The quintic polynomial interpolation is specifically: θ(t)=a 0 +a 1 t+a 2 t 2 +a 3 t 3 +a 4 t 4 +a 5 t 5 (4-21) The constraints are: It can be solved as: a 0 =θ 0 (11) in, t 0 The angular velocity at t f The angular acceleration at 5 is a constant parameter, a 4 is a constant parameter.

6. According to claim 4, a method for planning the operation path of a stubble robot arm, It is characterized in that The Cartesian space trajectory planning specifically includes linear interpolation and circular interpolation; The linear interpolation is specifically implemented in Cartesian space from the starting point P 1 (x 1 ,y 1 ,z 1 ) to the target point P 2 (x 2 ,y 2 ,z 2 )’s straight line trajectory planning; P 1 (x 1 ,y 1 ,z 1 ) to P 2 (x 2 ,y 2 ,z 2 ) can be expressed as: The interpolation of N points is completed at equal intervals. The coordinates of the kth point P' moved to in time t after a single interpolation segment distance d can be expressed as: Where L is the distance from P1 to P2, Assume λ = i / d, the maximum uniform speed is V max , acceleration is a, acceleration time is T m , acceleration section displacement L m , T m , L m It can be expressed as: Assume the acceleration time is T b , uniform time T a It can be expressed as: The total movement time T can be expressed as: Normalize the displacement, velocity, and acceleration of the parabola fitting segment: L mλ =L m / T,T mλ =T m / T,a λ =2L mλ / T mλ 2 , we can get: When λ=0, it is the starting point, and λ=1, it is the target point, so the position of each interpolation point can be calculated.

7. According to claim 5, a method for planning the operation path of a stubble robot arm, It is characterized in that The arc interpolation is specifically as follows: three non-collinear points P in Cartesian space 1 (x 1 ,y 1 ,z 1 ), P 2 (x 2 ,y 2 ,z 2 ), P 3 (x 3 ,y 3 ,z 3 ) can determine the circle, transform the generated arc to the plane, and then transform it into Cartesian space interpolation coordinates after interpolation; P 1 (x 1 ,y 1 ,z 1 ), P 2 (x 2 ,y 2 ,z 2 ), P 3 (x 3 ,y 3 ,z 3 ) The plane can be expressed as: A 1 x+B 1 y+C 1 z+D 1 =0 means the plane can get the coefficients: P 1 (x 1 ,y 1 ,z 1 ) 2 (x 2 ,y 2 ,z 2 ) perpendicular plane S 1 It can be expressed as: A 2 x+B 2 y+C 2 z+D 2 =0 means S 1 The coefficients of the plane are: Similarly, A 3 x+B 3 y+C 3 z+D 3 =0 means P 2 (x 2 ,y 2 ,z 2 ) 3 (x 3 ,y 3 ,z 3 ) perpendicular plane S 2 for: By combining the plane equations, we can get: Let the coordinates of the arc center be Q(x 0 ,y 0 ,z 0 ), the coordinates can be expressed as: The radius size is: QP 1 The u-axis is the arc normal vector, the w-axis is the arc normal vector, and the v-axis is created by the right-hand rule to establish the coordinate system. The u-axis unit vector can be expressed as: The w-axis unit vector is obtained: in: The transformation matrix of the original coordinate system to the coordinate system with Q as the base point can be expressed as: Assume the interpolation point P' = (u, v, w), where the central angle is ω, P 1 (x 1 ,y 1 ,z 1 ) 3 (x 3 ,y 3 ,z 3 ) The center angle of the arc connecting the lines is θ, then the number of interpolation can be expressed as: Then the coordinates of the i-th interpolation point can be expressed as: The transformation matrix converted to the original coordinate system can be expressed as: According to inverse kinematics, the inverse solution of each interpolation point is obtained to complete the Cartesian space arc interpolation trajectory planning.

8. A path planning system for a stubble robot arm. It is characterized in that The system uses the path planning method for a stubble robot arm operation as described in any one of claims 1 to 7, and the system includes: The stubble robot arm operation path optimization module performs path pruning and Bezier curve optimization on the stubble robot arm operation path to obtain a smooth stubble robot arm operation path; The trajectory planning module of the stubble robot arm is used to introduce the time dimension to calculate the trajectory planning based on the smooth operation path of the stubble robot arm; to achieve smooth operation and complete continuous posture transformation.

9. A computer device, It is characterized in that The method comprises a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, the method according to any one of claims 1 to 7 is implemented.

10. A computer-readable storage medium, It is characterized in that The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the method according to any one of claims 1 to 7 is implemented.

Citation Information

Patent Citations

  • Industrial robot trajectory optimization method based on improved genetic algorithm

    CN112692826A

  • Mechanical arm path planning method based on hybrid algorithm

    CN113858210A

  • Robot path planning method based on potential function

    CN114161416A

  • Mechanical arm multi-target trajectory planning method based on NSGA-III optimization algorithm

    CN114310899A

  • Drilling and anchoring robot drill arm track planning method and system and electronic equipment

    CN116276955A

Cited By

  • Multi-mechanical-arm surgical robot control method and device

    CN119858161A