Trajectory planning method for large redundant manipulator considering energy consumption and motion performance

Abstract

The application discloses a large-scale redundant mechanical arm trajectory planning method considering energy consumption and motion performance. The application firstly obtains the relative position of the mechanical arm end point relative to the mechanical arm base; then the working path of the mechanical arm end point is decomposed into multiple planning intervals with the same distance, so that trajectory planning is performed on each planning interval, the corresponding optimal attitude is obtained and used as the initial attitude of the next planning interval, and then all the attitudes of each planning interval are obtained, thereby realizing trajectory planning of the mechanical arm. The application is used for trajectory planning of a large-scale redundant mechanical arm which is insufficient in energy supply to support all joints to work simultaneously, has huge working energy consumption and poor motion stability. Under the energy consumption constraint and the kinematics constraint, the application can plan an optimal trajectory for any path of the mechanical arm under the energy saving index and the motion stability index, and can realize the working state switching between arms, and has good engineering application value.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of motion control, and particularly relates to a large redundant manipulator trajectory planning method considering energy consumption and motion performance. BACKGROUND

[0002] With the rapid development of China's economy, large manipulators play an increasingly important role in China's infrastructure and commercial buildings. The motion posture of the manipulator is mostly controlled by the operator using a joystick, which requires high experience and is prone to errors. In recent years, with the increasing demand for work, more and more large manipulators with five or six sections and a total length of more than fifty meters have appeared on the market. Since the large manipulator has redundant degrees of freedom when completing the work task, it needs to be converted into a multi-body nonlinear problem, and the manipulator posture is solved through inverse kinematics when the end point of the manipulator is known. In addition, the operation of the manipulator has different optimization objectives in different engineering scenarios, such as energy saving, obstacle avoidance, and path smoothing, and it is necessary to design a planning algorithm that meets the needs of different scenarios.

[0003] Agarwal et al. (Trajectory planning of redundant manipulator using fuzzy clustering method[J]. International Journal of Advanced Manufacturing Technology, 2012, 61(5-8): 727-744.) applied fuzzy clustering method to do trajectory planning of manipulator, and obtained manipulator posture data set to establish expert system, but this method is too dependent on the richness of data, and the system may not be able to complete the optimal planning task under various complex working conditions. Kang et al. (Trajectory Planning for Concrete Pump Truck based on Intelligent Hill Climbing and Genetic Algorithm[J], Applied Mechanics and Materials, 2012, 127: 360-367.) combined hill climbing algorithm and genetic algorithm to construct trajectory planning algorithm of manipulator under certain constraint conditions, and good results were obtained, but the calculation was complex, which made real-time control of manipulator difficult, and the calculation results of the algorithm in the motion process of the manipulator may appear non-convergence and mutation.

[0004] Under the actual working condition of a large mechanical arm, the mechanical arm has a serious "oil consumption" phenomenon due to the energy consumption of the driving mechanism, when the driving mechanism drives 6 arms at the same time, the mechanical arm cannot be accurately controlled due to insufficient oil supply, and the energy consumption of the mechanical arm is increased. Therefore, generally, it is set that at most 4 arms can work at the same time. How to make the mechanical arm find the most suitable posture to work in the motion trajectory under the existing motion constraint and energy saving requirement has become an urgent problem to be solved in the practical application of the mechanical arm. Therefore, the market demand for trajectory planning technology of a large redundant mechanical arm is great. SUMMARY

[0005] In order to solve the problems in the background art, the present application provides a large redundant mechanical arm trajectory planning method considering energy consumption and motion performance.

[0006] The technical scheme used by the present application is:

[0007] Step S1: obtaining the relative position of the mechanical arm end point with respect to the mechanical arm base;

[0008] Step S2: decomposing the working path of the mechanical arm end point into multiple planning intervals with the same distance, taking the planning interval where the mechanical arm end point is located as the initial planning interval, and selecting the four arms farthest from the mechanical arm base as the initial working arms, and keeping the other arms stationary;

[0009] Step S3: determining the weight of each working arm according to the initial posture of the mechanical arm in the current planning interval, and then constructing a weight matrix;

[0010] Step S4: based on the relative position of the mechanical arm end point with respect to the mechanical arm base, using the Newton iteration method to solve the feasible posture solution set of the mechanical arm end point moving to the other end point of the current planning interval, and then using the weight distribution method to calculate the optimal posture in the feasible posture solution set, judging whether the end point position corresponding to the current optimal posture satisfies the end point position error condition, if not, based on the current optimal posture and the corresponding end point position, using the Newton iteration method and the weight distribution method to calculate the optimal posture in the feasible posture solution set of moving to the other end point of the current planning interval, until the optimal posture satisfies the end point position error condition;

[0011] Step S5: judging whether the current optimal posture satisfies the working arm joint state constraint, if not, adjusting the weight matrix and repeating step S4 until the working arm joint state constraint is satisfied, obtaining the final optimal posture and the latest weight matrix, and taking the final optimal posture as the initial posture of the mechanical arm in the next planning interval;

[0012] Step S6: judging whether the weight of each working arm in the current weight matrix exceeds its preset weight threshold value, if the weight of a certain working arm exceeds its preset weight threshold value, updating the weight matrix and then executing step S7; otherwise, executing step S3 and then executing step S7;

[0013] Step S7: repeating steps S4-S6 to sequentially perform trajectory planning on the remaining planning intervals, obtaining the optimal poses of all planning intervals, and thus obtaining the optimal trajectory of the mechanical arm.

[0014] The step S1 is specifically:

[0015] The arm angle data is detected by a sensor installed on the mechanical arm, and the relative position of the end point of the mechanical arm relative to the base of the mechanical arm is calculated based on the arm angle data using forward kinematics.

[0016] The step S3 is specifically:

[0017] The joint angles of the current working arms are determined according to the initial pose of the mechanical arm in the current planning interval, the weights of the current working arms are determined according to the relationship between the joint angles of the current working arms and the corresponding angle threshold values, and then the weight matrix is constructed.

[0018] In the step S3, when the initial joint angle θ origin_i of the i-th working arm does not reach its preset angle threshold value θ threshold_i , the weight W i of the i-th working arm is determined according to the following formula:

[0019] W i = T i , i = 1…6

[0020] Wherein, T i is the weight parameter of the i-th arm;

[0021] When the initial joint angle θ origin_i of the i-th arm reaches or exceeds its preset angle threshold value θ threshold_i , the weight W i of the i-th working arm is determined according to the following formula:

[0022]

[0023] Wherein, S i is the deceleration parameter of the i-th arm, and θ boundary_i is the joint limit angle of the i-th arm.

[0024] In the step S4, the end point position error condition is specifically that the position error between the position of the end point and the position of the other end point of the current planning interval is calculated under the current optimal pose, and the position error is within the preset error range.

[0025] The step S5 is specifically:

[0026] S51: judging whether the joints of the current working arms exist reciprocating motion according to the current optimal pose, if the joints of a working arm exist reciprocating motion, fixing the joint angle of the working arm and then repeating step S4 until the joints of the current working arms do not exist reciprocating motion, and executing S52;

[0027] S52: judging whether the joint angles of the current working arms exceed the corresponding joint limiting angles according to the current optimal pose, if the joint angle of a working arm exceeds the corresponding joint limiting angle, stopping the joint motion of the working arm, stopping the motion of the working arm, deleting the weight of the working arm from the current weight matrix, updating the working arm, further updating the weight matrix, repeating steps S4 and S51 until the joint angles of the current working arms do not exceed the corresponding joint limiting angles, obtaining the final optimal pose and the latest weight matrix, and taking the final optimal pose as the initial pose of the robot arm in the next planning interval.

[0028] In the step S6, when the weight of a working arm exceeds the preset weight threshold, the motion of the working arm is stopped, the weight of the working arm is deleted from the current weight matrix, the working arm is updated, and the weight matrix is further updated.

[0029] In the step S6 or S52, the working arm is updated, specifically:

[0030] judging whether there is a working static arm in the robot arm, if yes, switching the farthest end of the static arm from the base to start working, otherwise directly updating the weight matrix according to the remaining working arms.

[0031] The beneficial effects of the present application are:

[0032] (1) The Newton iteration method is applied to solve the solution set of the nonlinear robot arm pose problem, and the solving speed is fast and the solving accuracy is high. In industry, the performance of the processor equipped in the robot arm is not high, and the application of the present application can make the real-time performance of the robot arm better.

[0033] (2) For the serious "oil consumption" phenomenon of large robot arms, low energy utilization rate and poor motion stability, the present application proposes an optimization solving strategy based on adaptive weight distribution to realize energy saving and consumption reduction and improve control stability, and finds the optimal solution under the current pose solution set according to the weight matrix.

[0034] (3) The present application considers the kinematic constraint and energy consumption constraint of the robot arm, and can realize step-by-step switching of the working joints when a joint of the robot arm reaches its angle threshold, and can drive part of the joints to work under the energy consumption constraint, which has good application prospect in actual working conditions. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 It is a schematic diagram of the robotic arm structure.

[0036] Figure 2 This is a flowchart of applying Newton iteration to solve the feasible posture of the robotic arm.

[0037] Figure 3 This is the overall flow chart of the robot arm trajectory planning algorithm proposed in this invention.

[0038] Figure 4 It is the trajectory planning result diagram of group a in the embodiment.

[0039] Figure 5 It is the trajectory planning result diagram of group b in the embodiment.

[0040] Figure 6 It is the trajectory planning result diagram of group c in the embodiment.

[0041] Figure 7 It is a practical flow chart of the present invention in actual working conditions. DETAILED DESCRIPTION

[0042] The present invention is further described in detail below with reference to the accompanying drawings and embodiments.

[0043] The structural diagram of a large redundant robotic arm is as follows: Figure 1 As shown in Figure 1, it consists of 6 arms that move in a plane and are connected in sequence by joints, and a base. The overall process of the large redundant robotic arm trajectory planning algorithm is as follows: Figure 3 As shown in the figure, the process of applying Newton iteration to solve the feasible posture of the manipulator is as follows Figure 2 shown.

[0044] The specific technical solutions of the present invention are:

[0045] Step S1: Obtain the relative position of the end point of the robotic arm with respect to the robotic arm base;

[0046] Step S1 is specifically as follows:

[0047] The angle data of each arm is obtained by detecting the sensors installed on the robot arm, and the relative position of the end point of the robot arm with respect to the base of the robot arm is obtained by forward kinematics calculation based on the angle data of each arm;

[0048] Specifically:

[0049] For a large redundant manipulator consisting of multiple arms that move in a plane and are connected by joints in sequence, and a rotating base, the center of the manipulator base is set to the origin O = [0 0 0] T , the relative position of the end point of the manipulator to the base of the manipulator is P, satisfying P = [px p y p z ] T , T is the matrix transpose, the length of the i-th arm of the robot is l i , i=1...6, the angle between the i-th arm and the i-1-th arm is θ i , then the three-dimensional coordinates p of the end point of the robot arm relative to the base x 、p y 、p z It can be calculated by the following formula:

[0050]

[0051] Where c0 is cos(θ0), s0 is sin(θ0), c1 is cos(θ1), s1 is sin(θ1), c12 is cos(θ1+θ2), s12 is sin(θ1+θ2), and so on; θ0 is the rotation angle of the robot base, with 0 degrees being the positive direction of the X-axis.

[0052] Since the robot arm mainly works in planar motion, the rotation angle θ0 of the robot arm base is a constant. Therefore, the position of the end point of the robot arm during movement is mainly determined by the angle between the arms, that is, the joint angle θ i (i=1...6) is determined. Therefore, formula (1) can be simplified as

[0053] P=F(Θ) (2)

[0054] Among them, Θ is the i (i=1…6) composed of joint angle vectors.

[0055] by Figure 1 Taking a large redundant manipulator with the structure shown in the figure as an example, its kinematic parameters are shown in Table 1. The initial angle of the manipulator joint is Θ initial =[75° 140° 150° 150° 130° 90°] T , the end point coordinates are (28048,3685,0). The angle threshold of each joint is set to a position 10° away from the joint's own angle limit.

[0056] Table 1 is the kinematic parameters of the large redundant manipulator

[0057]

[0058] Step S2: Decompose the working path of the end point of the robot arm into multiple planning intervals of equal distance. The planning interval where the end point of the robot arm is located is used as the initial planning interval. The four-segment arm farthest from the robot arm base is selected as the initial working arm. The other arms are temporarily kept stationary and serve as working stationary arms.

[0059] Step S3: determining the weight of each working arm according to the initial pose of the mechanical arm in the current planning interval, and then constructing a weight matrix;

[0060] Step S3 is specifically:

[0061] determining the joint angle of each working arm according to the initial pose of the mechanical arm in the current planning interval, determining the weight of each working arm according to the relationship between the joint angle of each working arm and the corresponding angle threshold, and then constructing a weight matrix;

[0062] In the process of trajectory planning of the motion of the mechanical arm, the energy consumption of the mechanical arm corresponding to the change of each joint angle is not the same; the energy consumption of the joint close to the base is much greater than that of the joint far from the base for the same angle change, so different weights are given to different joints to reflect the degree of influence on energy consumption.

[0063] In step S3, when the initial joint angle θ origin_i of the i-th working arm does not reach its preset angle threshold θ threshold_i , the weight W i of the i-th working arm is determined according to the following formula:

[0064] W i = T i , i = 1…6 (3)

[0065] wherein, T i is the weight parameter of the i-th arm, which is set according to the influence of the joint motion of the i-th arm on energy consumption, and the greater the energy consumption of the joint, the greater the corresponding weight parameter.

[0066] When the initial joint angle θ origin_i of the i-th arm reaches or exceeds its preset angle threshold θ threshold_i , the weight W i of the i-th working arm is determined according to the following formula:

[0067]

[0068] wherein, S i is the preset deceleration parameter of the i-th arm, and θ boundary_i is the joint limit angle of the i-th arm.

[0069] The joint angles of each arm segment are prone to oscillation as they approach their joint limits. Therefore, a corresponding angle threshold is set to proactively intervene in joint angle changes. The angle threshold can be set slightly lower than the limit angle. When the working joint angle reaches the threshold, the weighting of the working arm's joint angle changes is adjusted. When the joint angle of a working arm segment enters the threshold range, the corresponding joint weight value is adaptively increased to gradually slow and stop the movement of that joint.

[0070] According to the weight W set for each working arm joint i , construct the corresponding weight matrix. For example, the initial working arm is the four-joint arm at the distal end, and the subscripts of the working arms are represented as j, k, m, and n respectively. The formula of the 4×4 weight matrix W corresponding to the motion joint is:

[0071]

[0072] Step S4: Based on the relative position of the end point of the manipulator with respect to the base of the manipulator, the Newton iteration method is used to solve the feasible posture solution set of the manipulator end point moving from one end point of the current planning interval to the other end point of the current planning interval, and then the weight distribution method is used to calculate the optimal posture in the feasible posture solution set, and it is determined whether the end point position corresponding to the current optimal posture meets the end point position error condition. If not, based on the current optimal posture and the corresponding end point position, the Newton iteration method and the weight distribution method are used to calculate the optimal posture in the feasible posture solution set for moving to the other end point of the current planning interval until the optimal posture meets the end point position error condition;

[0073] Specifically:

[0074] S41: Determine the correlation between joint angle changes and end position changes

[0075] Taking the partial derivative of formula (2) with respect to time, we can get:

[0076]

[0077]

[0078] in, is the velocity matrix of the end point of the robot arm, is the angular velocity matrix of the robot arm joint, and J(Θ) represents the 3×6 Jacobian matrix of the transformation relationship from the joint angular velocity to the end point velocity under the current joint angle vector Θ.

[0079] Let J + (Θ) is the pseudo-inverse matrix of J(Θ), and the formula is as follows:

[0080] J + (Θ)=JT (Θ) (J(Θ) J T (Θ)) -1 (8)

[0081] From equation (6) and equation (8), we have:

[0082]

[0083] For the initial working arm being the distal end of the 4-joint arm, the subscripts of the working arm are represented as j, k, m, n respectively, and the Jacobian matrix of the initial working arm can be rewritten accordingly as:

[0084]

[0085] S42: solving the mechanical arm pose solution set by Newton iteration method

[0086] The Newton iteration equation for solving the joint angle vector Θ is:

[0087] Θ i =Θ i-1 +J + (Θ i-1 )error i-1 (11)

[0088] error i =P goal -P i (12)

[0089] Wherein, error i is the end point position error after the i-th iteration, Θ i is the calculation result of the joint angle vector of the i-th iteration, P goal is the target position of the end point, P i is the end point position calculated according to the calculation result of the joint angle vector of the i-th iteration Θ i after the i-th iteration, and the iteration result is compared with the error threshold value ξ in terms of the two norm ||error i ||2 of the position error to determine whether the iteration result meets the accuracy requirement. The flow chart of Newton iteration is shown in Figure 2 .

[0090] Since the redundant mechanical arm has redundant degrees of freedom, there are infinite solutions for the feasible pose of the actual mechanical arm end point moving to the other end point of the interval, so it is necessary to find the pose solution set under the target position of the end point, and then set the constraint condition and optimization target to find the optimal pose solution from the solution set that meets the condition. That is

[0091] ΔΘ=J + (Θ NI )ΔP+(I-J + (ΘNI )J(Θ NI ))z (13)

[0092] Θ op =Θ origin +ΔΘ (14)

[0093] where ΔΘ is the joint angle change of the redundant manipulator, ΔP is the error between the initial position and the target position of the end point of the redundant manipulator currently planned, Θ NI is the feasible pose solution obtained by Newton iteration, z is the vector to be optimized, Θ origin is the initial joint angle vector of the planning interval of the manipulator, Θ op is the joint angle vector of the manipulator corresponding to the target position of the end point. Θ op will change with the change of z, and the different Θ op obtained thereby constitute the pose solution set space of the manipulator under the target position of the end point.

[0094] S43: Optimal pose solution of the manipulator based on weight distribution

[0095] Based on the consideration of reducing the energy consumption of the motion of the manipulator, the following loss function Q is proposed, and the formula is:

[0096]

[0097] where W i is the weight value given to the joint of the i-th section arm in step S3, and Δθ i is the joint angle change of the i-th section arm.

[0098] By simultaneously solving equations (1)-(15), it can be obtained that the loss function Q is a function of the vector z, and therefore finding the optimal pose solution minimizing Q is equivalent to solving the following equation:

[0099]

[0100] The optimal z op solved is:

[0101] z op =A -1 B (17)

[0102] where A=(I-J + J) T W(I-J + J), and B=-(I-J + J) T WJ +ΔP, W is a weight matrix composed of weights corresponding to the current working arm corresponding motion joint, and ΔP is the error of the mechanical arm end point in the initial position and the target position of the current planning interval.

[0103] S44: the solved z op is substituted into formula (13) and formula (14) to solve the corresponding ΔΘ and the current optimal Θ op . According to the obtained Θ op , the corresponding end point position is calculated as the initial position of the current planning, the error ΔP is updated, and the two norm ||ΔP||2 of the error is compared with the error threshold ξ to determine whether the optimal pose of the current planning meets the position accuracy requirement. If not, the end point position of the current optimal pose is used to continue to repeat steps S42 and S43 until the optimal Θ op is obtained as the optimal pose of the mechanical arm at the target position of the end point.

[0104] Step S5: determine whether the current optimal pose meets the working arm joint state constraint. If not, adjust the weight matrix and repeat step S4 until the working arm joint state constraint is met, and obtain the final optimal pose and the latest weight matrix. The final optimal pose is used as the initial pose of the mechanical arm in the next planning interval.

[0105] Step S5 is specifically:

[0106] S51: according to the current optimal pose, determine whether the joints of the current working arm exist reciprocating motion. If the joint of a certain working arm exists reciprocating motion, i.e. the motion direction of the joint changes, then fix the joint angle of the working arm and repeat step S4 until the joints of the current working arm do not exist reciprocating motion, and S52 is executed.

[0107] S52: according to the current optimal pose, determine whether the joint angle of the current working arm exceeds the corresponding joint limit angle. If the joint angle of a certain working arm exceeds the corresponding joint limit angle, stop the motion of the joint corresponding to the working arm, stop the motion of the working arm, delete the weight of the working arm from the current weight matrix, and the working arm is used as a static arm that does not work. The corresponding joints in the subsequent planning interval do not move. The working arm is updated, and then the weight matrix is updated. Repeat steps S4 and S51 until the joint angle of the current working arm does not exceed the corresponding joint limit angle. The current optimal pose meets the end point position error condition and the working arm joint state constraint, and the final optimal pose and the latest weight matrix are obtained. The final optimal pose is used as the initial pose of the mechanical arm in the next planning interval.

[0108] Step S6: Determine whether the weight of each working arm in the current weight matrix exceeds its preset weight threshold. If the weight of a working arm exceeds its preset weight threshold, update the weight matrix and then execute step S7; otherwise, execute step S3 and then step S7;

[0109] In step S6, when the weight of a working arm exceeds its preset weight threshold, the movement of the working arm is stopped, the weight of the working arm is deleted from the current weight matrix, and the working arm is treated as a non-working static arm. The corresponding joints in the trajectory planning of the subsequent planning interval do not move, the working arm is updated, and then the weight matrix is ​​updated.

[0110] In step S6 or S52, the working arm is updated, specifically:

[0111] Determine whether there is a working stationary arm in the robot arm. If so, switch the stationary arm farthest from the base to start working, and update the weight matrix according to the current working arms. Otherwise, directly update the weight matrix and subsequent trajectory planning based on the remaining working arms.

[0112] Step S7: Repeat steps S4-S6, perform trajectory planning for the remaining planning intervals in turn, obtain the optimal posture of all planning intervals, and thus obtain the optimal trajectory of the robotic arm.

[0113] The embodiments are divided into three groups: a. using only Newton iteration method for trajectory planning, b. using fixed weights for trajectory planning (the weights of joints 1 to 6 are set to 6, 5, 4, 3, 2, and 1, respectively), and c. using the method of the present invention for trajectory planning, with the motion path set to 10,000 mm along the positive direction of the X-axis.

[0114] The motion trajectory planning results of the three groups are as follows: Figure 4 、 Figure 5 、 Figure 6 shown.

[0115] Evaluation indicators E1 and E2 for energy consumption and motion smoothness are proposed respectively.

[0116]

[0117] Where Z is the length of the working path, t (t = 1...N) represents the t-th working interval, i (i = 1...6) represents the i-th arm, J i is the moment of inertia of the i-th arm, ω i,t and ω i,t-1 are the angular velocities of the i-th joint in the t-th and t-1-th working intervals respectively.

[0118] E2=maxp i

[0119] Among them, p i is the average angular acceleration of the i-th joint.

[0120] The comparison of evaluation results is shown in Table 2.

[0121] Table 2 is the evaluation results table

[0122]

[0123] As shown in Table 2 and Figure 4 、 Figure 5 、 Figure 6 As shown, when only fixed weights are used for path planning, all six arms move simultaneously, and the weights cannot be adjusted adaptively. This results in high energy consumption and poor stability, making it difficult to apply in practical engineering. When Newton iteration is combined with adaptive weight distribution, the algorithm can better distribute the workload of each joint and achieve automatic switching of the working arms, with minimal energy consumption and the easiest control. The simulation results show that this invention can achieve excellent trajectory planning for large redundant robotic arms. Figure 7 This is a practical flow chart of the present invention in actual working conditions.

[0124] In summary, the method proposed in the present invention for trajectory planning of a large robotic arm with redundant degrees of freedom under energy-saving indicators and kinematic constraints is effective and feasible, and can well meet the trajectory planning requirements when the redundant robotic arm consumes huge energy, has poor controllability, and cannot support all joints working simultaneously.

[0125] The above examples are merely provided to further illustrate the technical content of the present invention and facilitate understanding by those skilled in the art. However, they do not limit the present invention to these examples. Any extension or re-creation of the technology based on the present invention is protected by the present invention. The scope of protection of the present invention shall be determined by the claims.

Claims

1. A large redundant robotic arm trajectory planning method considering energy consumption and motion performance, characterized in that: The following steps are involved: Step S1: Obtain the relative position of the end point of the robotic arm with respect to the robotic arm base; Step S2: Decompose the working path of the end point of the robot arm into multiple planning intervals of equal distance. The planning interval where the end point of the robot arm is located is used as the initial planning interval. The four-segment arm farthest from the robot arm base is selected as the initial working arm, and the other arms remain stationary. Step S3: Determine the weight of each working arm according to the initial posture of the robot arm in the current planning interval, and then construct a weight matrix; Step S4: Based on the relative position of the end point of the manipulator with respect to the base of the manipulator, the Newton iteration method is used to solve the feasible posture solution set for the end point of the manipulator to move to the other end point of the current planning interval, and then the weight distribution method is used to calculate the optimal posture in the feasible posture solution set, and it is determined whether the end point position corresponding to the current optimal posture meets the end point position error condition. If not, based on the current optimal posture and the corresponding end point position, the Newton iteration method and the weight distribution method are used to calculate the optimal posture in the feasible posture solution set for moving to the other end point of the current planning interval until the optimal posture meets the end point position error condition; Step S5: Determine whether the current optimal posture satisfies the working arm joint state constraints. If not, adjust the weight matrix and repeat step S4 until the working arm joint state constraints are satisfied. Obtain the final optimal posture and the latest weight matrix, and use the final optimal posture as the initial posture of the robotic arm in the next planning interval. Step S6: Determine whether the weight of each working arm in the current weight matrix exceeds its preset weight threshold. If the weight of a working arm exceeds its preset weight threshold, update the weight matrix and then execute step S7; Otherwise, execute step S3 and then execute step S7; Step S7: Repeat steps S4-S6, perform trajectory planning for the remaining planning intervals in turn, obtain the optimal posture of all planning intervals, and thus obtain the optimal trajectory of the robotic arm.

2. A large redundant robotic arm trajectory planning method considering energy consumption and motion performance according to claim 1, characterized in that: The step S1 is specifically as follows: The angle data of each arm is obtained by detecting the sensors installed on the robot arm, and the relative position of the end point of the robot arm with respect to the robot arm base is obtained based on the angle data of each arm using forward kinematics calculation.

3. A large-scale redundant manipulator trajectory planning method considering energy consumption and motion performance according to claim 1, characterized in that: The step S3 is specifically as follows: The joint angles of the current working arms are determined according to the initial posture of the robotic arm in the current planning interval. The weights of the current working arms are determined according to the relationship between the joint angles of the current working arms and the corresponding angle thresholds, and then a weight matrix is ​​constructed.

4. A large-scale redundant manipulator trajectory planning method considering energy consumption and motion performance according to claim 3, characterized in that: In step S3, when the initial angle θ of the joint of the i-th working arm is origin_i The preset angle threshold θ is not reached threshold_i When the weight W of the i-th working arm is i The formula is as follows: W i =T i ,i=1…6 Among them, T i is the weight parameter of the i-th arm; When the initial joint angle θ of the i-th arm origin_i Reach or exceed its preset angle threshold θ threshold_i When the weight W of the i-th working arm is i The formula is as follows: Among them, S i is the deceleration parameter of the i-th arm, θ boundary_i is the joint limit angle of the i-th arm.

5. The large-scale redundant manipulator trajectory planning method considering energy consumption and motion performance according to claim 1 is characterized in that: In step S4, satisfying the end point position error condition specifically means calculating the position error between the position of the end point and the other end point of the current planning interval under the current optimal posture, and the position error is within a preset error range.

6. A large redundant manipulator trajectory planning method considering energy consumption and motion performance according to claim 1, characterized in that: The step S5 is specifically as follows: S51: Determine whether the joints of the current working arms have reciprocating motion based on the current optimal posture. If a joint of a working arm has reciprocating motion, fix the joint angle of the working arm and repeat step S4 until the joints of the current working arms no longer have reciprocating motion, and then execute S52; S52: Determine whether the joint angles of the current working arms exceed their corresponding joint limit angles based on the current optimal posture. If the joint angles of a working arm exceed their corresponding joint limit angles, stop the joint movement corresponding to the working arm, delete the weight of the working arm from the current weight matrix, update the working arm, and then update the weight matrix. Repeat steps S4 and S51 until the joint angles of the current working arms do not exceed their corresponding joint limit angles, obtain the final optimal posture and the latest weight matrix, and use the final optimal posture as the initial posture of the robotic arm for the next planning interval.

7. A large redundant manipulator trajectory planning method considering energy consumption and motion performance according to claim 1, characterized in that: In step S6, when the weight of a working arm exceeds its preset weight threshold, the movement of the working arm is stopped, the weight of the working arm is deleted from the current weight matrix, the working arm is updated, and then the weight matrix is ​​updated.

8. A large redundant manipulator trajectory planning method considering energy consumption and motion performance according to claim 6 or 7, characterized in that: In step S6 or S52, the working arm is updated, specifically: Determine whether there is a workable static arm in the robot arm. If so, switch the static arm farthest from the base to start working. Otherwise, directly update the weight matrix according to the remaining working arms.

Images