Method for determining position of tail end of rigid-flexible coupling arm and working machine

Abstract

The invention relates to a method for determining the position of the tail end of a rigid-flexible coupling arm and an operation machine. The method comprises the steps that the current joint position and angular speed of each joint arm frame of a mechanical arm are obtained; based on the dynamic model of the rigid joint arm support, determining a joint position of each joint arm support and a driving torque corresponding to an angular velocity; determining a first position of the tail end of the mechanical arm based on the joint position of each joint arm support; based on a mechanical model of the flexible joint arm frame, the deflection of the tail end of the mechanical arm is determined according to the driving torque of each joint arm frame; and the first position is corrected according to the deflection, and the second position of the tail end of the mechanical arm is determined. When the position of the tail end of the mechanical arm is determined, the influence of the driving torque corresponding to the angular velocity of the joint arm frame on the deflection of the tail end of the mechanical arm is combined, so that the position calculation result of the tail end of the mechanical arm better conforms to the real-time movement characteristics of the mechanical arm, and the solving precision of the position of the tail end of the mechanical arm is improved; and the real-time requirement of tail end motion estimation is better met.

Description

Technical Field

[0001] The present application relates to the technical field of mechanical equipment, and in particular to a method for determining the position of an end of a rigid-flexible coupling arm and an operating machine. Background Art

[0002] The high degree of rigid-flexible coupling of the boom system makes it difficult to estimate the dynamic state of boom-type operating machinery. The existing technology mainly uses dynamic simulation technology based on high-density grids and dynamic state estimation technology based on segmented elastic body models. However, these methods mainly estimate the dynamic state based on the position information of the boom, and the real-time calculation error of the end position is large, and it is difficult to meet the requirements of real-time control. Summary of the invention

[0003] In response to the above technical problems, the present application provides a method, an operating machine, an electronic device and a storage medium for determining the position of the end of a rigid-flexible coupling arm, which can improve the accuracy of solving the position of the end of the robot arm and better meet the real-time requirements of end motion estimation.

[0004] In order to solve the above technical problems, the present application provides a method for determining the position of the end of a rigid-flexible coupling arm, the method comprising the following steps: Obtaining the current joint position and angular velocity of each joint arm of the robotic arm; Determine the driving torque corresponding to the joint position and the angular velocity of each joint arm based on a dynamic model of the rigid joint arm; and determine the first position of the end of the mechanical arm based on the joint position of each joint arm; Based on the mechanical model of the flexible joint arm, the deflection of the end of the mechanical arm is determined according to the driving torque of each joint arm; The first position is corrected according to the deflection to determine a second position of the end of the robot arm.

[0005] In some embodiments, the mechanical model based on the flexible joint arm frame determines the deflection of the end of the mechanical arm according to the driving torque of each joint arm frame, including: Determine the external force on each joint arm frame according to the driving torque of each joint arm frame; Based on the mechanical model of the flexible joint arm, the deflection of the end of each joint arm is determined according to the external force applied to each joint arm; The deflection of the end of the robot arm is determined according to the deflection of the end of each joint arm frame.

[0006] In some embodiments, the mechanical model based on the flexible joint arm frame determines the deflection of the end of each joint arm frame according to the external force applied to each joint arm frame, including: Based on the mechanical model of the flexible joint arm, determine the first deflection of the cross section of the corresponding joint arm caused by the bending moment of the equivalent force of the rear end arm of each joint arm, the second deflection of the cross section of the corresponding joint arm caused by the external force on each joint arm, and the third deflection of the cross section of the corresponding joint arm caused by the self-weight of each joint arm; The deflections of the ends of the respective articulated arms are determined respectively according to the first deflection, the second deflection and the third deflection of the respective articulated arms.

[0007] In some embodiments, after the first position is corrected according to the deflection to determine the second position of the end of the robotic arm, the method further includes one of the following: Compensating the second position of the end of the robotic arm based on an end position compensation model, wherein the end position compensation model is trained based on the second position and actual position of the end of the robotic arm at different times; Correcting the second position of the end of the robotic arm based on a first online correction algorithm; The second position of the end of the robot arm is compensated based on the end position compensation model, and the compensated second position is corrected based on a second online correction algorithm.

[0008] In some embodiments, the step of training the end position compensation model includes: Obtaining a set of motion trajectories of the end of the robotic arm; Controlling the end of the robotic arm to move at different speeds based on the motion trajectory set, and collecting the actual position of the end of the robotic arm at different times and the joint positions and angular velocities of each joint arm frame at different times; Determine the second position of the end of the robotic arm at different times according to the joint positions and angular velocities of the joint arms at different times, and determine the position difference between the second position and the actual position at the same time; Generate a training data set, each set of training data in the training data set includes the joint positions and angular velocities of each joint arm frame corresponding to the same moment, and the position difference; A preset neural network model is trained based on the training data set to obtain the end position compensation model, wherein the input of the end position compensation model is the current joint position and angular velocity of each joint arm, and the output is a compensation value for compensating for the second position of the end of the robotic arm, and the compensation value is used to characterize the position difference between the current second position of the end of the robotic arm and the actual position.

[0009] In some embodiments, obtaining a set of motion trajectories of the end of the robotic arm includes: Determining a set of position points of the end of the robotic arm according to the movable range of each joint arm frame; Determining boundary points in the set of position points; Determine the polygon boundary according to the boundary points; Within the movable range formed by the polygonal boundary, motion trajectories of the end of the robot arm in different motion directions are planned to obtain the motion trajectory set.

[0010] In some embodiments, planning the motion trajectory of the end of the robot arm in different motion directions within the movable range formed by the polygonal boundary to obtain the motion trajectory set includes: Drawing a dividing line with a concave point in the polygonal boundary as a starting point, so as to divide the movable range formed by the polygonal boundary into convex polygonal areas through the dividing line; Planning a motion trajectory perpendicular or parallel to the dividing line in each of the convex polygonal regions at the same path interval; The dividing line is rotated by a preset angle, and a motion trajectory perpendicular to the dividing line is continuously planned in each of the convex polygonal regions at the same path interval until the dividing line is rotated to an initial position; All planned motion trajectories are acquired to obtain the motion trajectory set.

[0011] In some embodiments, the step of obtaining the algorithm parameters of the first online correction algorithm includes: Taking the actual position of the end of the robotic arm as a first observation value and the second position of the end of the robotic arm as a first prediction value, calculating algorithm parameters of the first online correction algorithm so that an error between the first observation value and the first prediction value meets a first preset condition; And / or, the step of acquiring the algorithm parameters of the second online correction algorithm includes: Taking the actual position of the end of the robotic arm as the second observation value and the compensated second position of the end of the robotic arm as the second prediction value, the algorithm parameters of the second online correction algorithm are calculated so that the error between the second observation value and the second prediction value meets the second preset condition.

[0012] The present application also provides a working machine, including a controller and a mechanical arm, wherein the mechanical arm includes a plurality of joint arms, and when the controller executes a computer program, the method for determining the position of the end of the rigid-flexible coupling arm as described above is implemented. The present application also provides an electronic device, including a storage medium and a controller, wherein a computer program is stored on the storage medium, and when the computer program is executed by the controller, the method for determining the position of the end of the rigid-flexible coupling arm as described above is implemented.

[0013] The present application also provides a storage medium having a computer program stored thereon. When the computer program is executed by a processor, the method for determining the position of the end of the rigid-flexible coupling arm as described above is implemented.

[0014] The present application discloses a method and operating machine for determining the position of the end of a rigid-flexible coupling arm, the method comprising: obtaining the current joint position and angular velocity of each joint arm of the robot arm; determining the driving torque corresponding to the joint position and angular velocity of each joint arm based on the dynamic model of the rigid joint arm; determining the first position of the end of the robot arm based on the joint position of each joint arm; determining the deflection of the end of the robot arm according to the driving torque of each joint arm based on the mechanical model of the flexible joint arm; and correcting the first position according to the deflection to determine the second position of the end of the robot arm. When determining the position of the end of the robot arm, the present application combines the influence of the driving torque corresponding to the angular velocity of the joint arm on the deflection of the end of the robot arm, so that the position calculation result of the end of the robot arm is more in line with the characteristics of the real-time motion of the robot arm, improves the solution accuracy of the position of the end of the robot arm, and better meets the real-time requirements of the end motion estimation. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] Figure 1 is a flow chart of a method for determining a position of an end of a rigid-flexible coupling arm according to an embodiment.

[0016] Figure 2 It is a schematic diagram of the principle of deformation of the robotic arm.

[0017] Figure 3 It is a schematic diagram of the principle of deformation of the articulated arm.

[0018] Figure 4 is a schematic diagram of a set of position points at the end of a robotic arm according to an embodiment.

[0019] Figure 5 is a schematic diagram showing a movable range boundary obtained by curve fitting according to an embodiment.

[0020] Figure 6 According to an embodiment, Figure 5 Schematic diagram of the polygonal boundary obtained by approximating the boundary in .

[0021] Figure 7 It is a schematic diagram showing motion trajectory planning within a movable range formed by a polygonal boundary according to an embodiment. DETAILED DESCRIPTION

[0022] In order to facilitate the understanding of the present application, the present application will be described more fully below with reference to the relevant drawings. The preferred embodiments of the present application are given in the drawings. However, the present application can be implemented in many different forms and is not limited to the embodiments described herein. On the contrary, the purpose of providing these embodiments is to make the disclosure of the present application more thoroughly and comprehensively understood.

[0023] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as those generally understood by those skilled in the art to which this application belongs. The terms used herein in the specification of this application are only for the purpose of describing specific embodiments and are not intended to limit this application. The term "and / or" used herein includes any and all combinations of one or more related listed items. In this application, "each" includes one and more than two quantities.

[0024] Figure 1 FIG. 1 is a flow chart of a method for determining the position of the end of a rigid-flexible coupling arm according to an embodiment. Figure 1 As shown, the method for determining the position of the end of the rigid-flexible coupling arm of the present application comprises the following steps: S1, obtaining the current joint position and angular velocity of each joint arm of the robot arm; S2, determining the driving torque corresponding to the joint position and angular velocity of each joint arm frame based on the dynamic model of the rigid joint arm frame; and determining the first position of the end of the robotic arm based on the joint position of each joint arm frame; S3, based on the mechanical model of the flexible joint arm, the deflection of the end of the robot arm is determined according to the driving torque of each joint arm; S4, correcting the first position according to the deflection to determine the second position of the end of the robot arm.

[0025] The mechanical arm is the rigid-flexible coupling arm, which includes a plurality of joint arms connected in sequence. The current joint position of the joint arm refers to the position of the joint arm when the mechanical arm is working, and the joint position of the joint arm can be characterized by a joint angle. The current angular velocity of the joint arm refers to the angular velocity of the joint arm when the mechanical arm is working, which is used to characterize the current movement speed of the joint arm.

[0026] The driving torque refers to the torque generated by the driving force of the articulated arm. The dynamic model of the rigid articulated arm is used to characterize the relationship between the joint position, angular velocity and driving torque of the articulated arm. The dynamic model of the rigid articulated arm can be obtained by, for example, deforming the Lagrangian function, which is an energy-based dynamic method. The difference between the kinetic energy E and the potential energy P of the mechanical system is the Lagrangian function. It should be noted that the method for obtaining the dynamic model used to characterize the relationship between the joint position, angular velocity and driving torque of the articulated arm is not limited to this.

[0027] Taking the dynamic model of the rigid joint arm as an example, the Lagrangian function is deformed by the Lagrangian function. The Lagrangian function expression is:

[0028] In the formula, is the joint position, is the angular velocity, is the kinetic energy of the articulated arm, is the potential energy of the joint arm. Taking the derivative of the above equation, we get the dynamic equation of motion, which is:

[0029] In the formula, is the driving torque of the articulated arm. For the articulated arm, the above formula can be further transformed into:

[0030] Arranged into a matrix form, the dynamic model of the rigid joint arm is obtained:

[0031] Where n is the degree of freedom of the joint arm; is the inertia matrix of the articulated arm; is the matrix of centrifugal and Coriolis force terms in the joint space; is the gravity matrix of the articulated arm; , is the driving torque of the articulated arm; They are the joint position, angular velocity and acceleration matrix of the joint arm respectively. The physical quantities in the above formula are related to the current motion state of the joint arm. The specific calculation process can be realized by existing technology and will not be repeated here.

[0032] The articulated arm has a certain flexibility, and its mechanical model can be approximated as a cantilever beam. Based on the elastic deformation and small deformation theory of the beam, the mechanical model of the flexible articulated arm can be established. Different angular velocities require different driving torques, which are provided by the hydraulic system and can be considered to be generated by the external force on the articulated arm. Under different driving torques, the deformation of the articulated arm is also different. Therefore, the mechanical model of the flexible articulated arm can be used to determine the deflection of the end of the robotic arm. Specifically, the deflection of the end of the robotic arm can be determined according to the driving torque of each articulated arm.

[0033] The first position of the end of the manipulator is determined based on the joint positions of each joint arm frame. That is, the first position of the end of the manipulator is obtained without considering the angular velocity of the joint arm frame, and thus can be obtained based on the joint positions using the prior art method. In some embodiments, the first position of the end of the manipulator can be calculated by the standard DH matrix of the forward kinematics of the manipulator. The standard DH matrix can be obtained by the coordinate transformation matrix relative to the base coordinates of the manipulator To express, that is:

[0034] in: , , , , , Wherein, i=0 represents the base, i=1 represents the turntable, i=2 represents the first joint arm, i-1 represents the i-1th joint arm, i is the end of the i-1th joint arm; L represents the length of the joint arm.

[0035] However, the robot arm moves in real time driven by the drive system. The end position determined only based on the position information of the joint arm belongs to a static position, which ignores the characteristics of the real-time movement of the robot arm. The real-time calculation error is large and it is difficult to meet the requirements of real-time control. In this application, the first position is corrected based on the deflection of the end of the robot arm to determine the second position of the end of the robot arm. Since the deflection of the end of the robot arm is determined based on the driving torque corresponding to the joint position and angular velocity of each joint arm, combined with the influence of the driving torque of the joint arm on the position of the dynamically moving end of the robot arm, the corrected second position is a dynamic position, so that the position calculation result of the end of the robot arm is more in line with the characteristics of the real-time movement of the robot arm, the solution accuracy of the position of the end of the robot arm is improved, and the real-time requirements of the end motion estimation are better met.

[0036] Taking the standard DH matrix to represent the first position as an example, is a 4x4 matrix, where the last element of the first row, the last element of the second row, and the last element of the third row correspond to the position matrix , when correcting the first position, move the end of the robot arm to The deflection in the direction is added to the last element of the first row, and the end of the robot arm is The deflection in the y direction is superimposed on the last element of the third row. Considering that the working machine is basically in a horizontal state when working (that is, the working machine is not in a tilted state), it can be considered that there is no error in the y direction, and the last element of the second row is not superimposed. The matrix is ​​superimposed to obtain the second position.

[0037] In some embodiments, step S2, based on the mechanical model of the flexible joint arm, determines the deflection of the end of the robot arm according to the driving torque of each joint arm, including: Determine the external force on each joint arm frame according to the driving torque of each joint arm frame; Based on the mechanical model of the flexible joint arm, the deflection of the end of each joint arm is determined according to the external force applied to each joint arm; The deflection of the end of the robot arm is determined according to the deflection of the end of each joint arm.

[0038] Among them, the relationship between the driving torque and external force of the joint arm can be expressed as ,in, is the external force on the joint arm, is the arm length of the articulated arm, is the driving torque corresponding to the articulated arm. It can be understood that the relationship between the driving torque of the articulated arm and the external force is not limited to the deformation of other formulas.

[0039] After calculating the deflection of the end of each joint arm, the deflection of the end of the robot arm is obtained based on the deformation superposition principle. Specifically, the deformation of each joint arm in the x and z axis directions is calculated separately, and then superimposed and added together in the x and z axis directions. Please refer to Figure 2 For a robot with n joint arms, the end of the robot is at The deflection in the direction is , The deflection in the direction is Considering that the operating machinery is basically in a horizontal state when working (that is, the operating machinery is not in a tilted state), it can be considered that no deflection occurs in the y direction.

[0040] In some embodiments, based on the mechanical model of the flexible joint arm, the deflection of the end of each joint arm is determined according to the external force applied to each joint arm, including: Based on the mechanical model of the flexible joint arm, determine the first deflection of the cross section of the corresponding joint arm caused by the bending moment of the equivalent force of the rear end arm of each joint arm, the second deflection of the cross section of the corresponding joint arm caused by the external force on each joint arm, and the third deflection of the cross section of the corresponding joint arm caused by the self-weight of each joint arm; The deflection of the end of each articulated arm is determined respectively according to the first deflection, the second deflection and the third deflection of each articulated arm.

[0041] Please refer to Figure 3 In one embodiment, in addition to considering the second deflection of the cross section of the corresponding articulated arm caused by the external force F of the articulated arm, the deflection of the end of the articulated arm can also be further superimposed with the first deflection of the cross section of the corresponding articulated arm caused by the bending moment M of the rear end arm equivalent of the articulated arm, and the third deflection of the cross section of the corresponding articulated arm caused by the self-weight G of each articulated arm. Here, the external force F involved in the deflection calculation is the external force that generates the driving torque, thereby reflecting the influence of the driving torque (or angular velocity) of the articulated arm on the end position of the dynamically moving articulated arm.

[0042] Taking the first joint arm of the robot arm as an example, the deflection of the cross section caused by the bending moment M of the rear arm is , the deflection of the cross section caused by the external force F is , the deflection of the cross section caused by its own gravity G is , so the deflection of the end of the first joint arm is Among them, the bending moment M can be calculated by combining the inclination angle of the articulated boom, the rear end boom section configuration, the mass and other parameters. The specific calculation process can be realized by existing technology. is the length of the first arm section, is the elastic modulus of the first arm segment, is the section moment of inertia of the first boom section, is the angle between the first boom section and the horizontal plane, Desirable .

[0043] Through the above method, the second position of the end of the robotic arm can be calculated as the theoretical value of the dynamic position, which improves the solution accuracy of the position of the end of the robotic arm and better meets the real-time requirements of the end motion estimation, which can be used as the basis for subsequent automatic operation control to improve operation efficiency and accuracy.

[0044] Since the cross-sections of the various joint arms of the robot arm are complex and there are inevitable manufacturing gaps at the joints, the end position calculated based on the above method may deviate from the actual position. Therefore, the second position can be further corrected to improve the accuracy of the end position of the robot arm.

[0045] In some embodiments, after step S3, correcting the first position according to the deflection and determining the second position of the end of the robotic arm, the method further includes one of the following: Compensating the second position of the end of the robotic arm based on an end position compensation model, wherein the end position compensation model is trained based on the second position and the actual position of the end of the robotic arm at different times; Correcting a second position of the end of the robotic arm based on a first online correction algorithm; The second position of the end of the robot arm is compensated based on the end position compensation model, and the compensated second position is corrected based on the second online correction algorithm.

[0046] Among them, the end position compensation model is trained based on the second position and actual position of the end of the manipulator at different times, and the second position can be compensated to make the compensated end position closer to the actual position and reduce the position deviation. In the stage of training the model, the second position can also be calculated using the method of steps S1 to S4, and the actual position can be collected by using a total station to automatically track the end position. Since the second position used to train the end position compensation model is a dynamic position, it is ensured that the end position compensation model is effective in compensating the second position during actual work.

[0047] The first online correction algorithm is used to directly correct the second position of the end of the robot arm. Considering the errors that may still exist after compensation using the end position compensation model, the second online correction algorithm can be further used to correct the second position after compensation based on the end position compensation model. The first online correction algorithm and the second online correction algorithm can be the same algorithm or different algorithms, such as edge particle filtering, unscented Kalman filtering, etc. When the first online correction algorithm and the second online correction algorithm are the same algorithm, the algorithm parameters used by each are different, so as to be respectively suitable for directly correcting the second position and the second position after compensation.

[0048] In some embodiments, the step of training the end position compensation model includes: S10, obtaining a set of motion trajectories of the end of the robotic arm; S20, controlling the end of the robot arm to move at different speeds based on the motion trajectory set, and collecting the actual position of the end of the robot arm at different times and the joint positions and angular velocities of each joint arm frame at different times; S30, determining a second position of the end of the robot arm at different times according to the joint positions and angular velocities of each joint arm frame at different times, and determining a position difference between the second position and the actual position at the same time; S40, generating a training data set, wherein each set of training data in the training data set includes the joint positions, angular velocities, and position differences of each joint arm corresponding to the same moment; S50, training a preset neural network model based on a training data set to obtain an end position compensation model, wherein the input of the end position compensation model is the current joint position and angular velocity of each joint arm, and the output is a compensation value for compensating for the second position of the end of the robotic arm, and the compensation value is used to characterize the position difference between the current second position of the end of the robotic arm and the actual position.

[0049] Among them, the motion trajectory set of the end of the robot arm is a set of motion trajectories determined within the movable range of the end of the robot arm. On this basis, the end of the robot arm is controlled to move at different speeds. During the movement, the actual position of the end of the robot arm at different times and the joint positions and angular velocities of each joint arm frame at different times are collected. The respective timestamps are determined during the collection to ensure the time synchronization of the collected data. Since the motion trajectory set can provide a variety of movement directions of the end of the robot arm, combined with the different speed movements of the end of the robot arm, it is possible to simulate as many movement conditions as possible within the movable range of the end of the robot arm, making the collected data more real and comprehensive, and realizing the automation of the collection process.

[0050] Next, according to the joint positions and angular velocities of each joint arm at different times, the second position of the end of the manipulator at different times can be determined based on the method of step S1 to step S4, and then the position difference between the second position and the actual position at the same time can be determined in combination with the actual position collected. After that, the data is sorted to generate a training data set, and each group of training data in the training data set includes the joint position, angular velocity, and position difference of each joint arm corresponding to the same time. The joint position and angular velocity of each joint arm at the same time are used as input, and the position difference at the same time is used as output to train a preset neural network model to obtain an end position compensation model. In this way, when using the end position compensation model, the input is the current joint position and angular velocity of each joint arm, and the output is the position difference between the current second position of the end of the manipulator and the actual position, that is, the compensation value used to compensate for the second position of the end of the manipulator. When the end position compensation model is applied, the compensation value output by the end position compensation model is superimposed on the second position of the end of the manipulator to achieve compensation for the second position.

[0051] The preset neural network model can select a suitable multi-input and multi-output neural network model, including but not limited to convolutional neural networks, autoencoder neural networks, modular neural networks, etc., and complete the model training by selecting the appropriate number of network layers and neurons.

[0052] In some embodiments, before training a preset neural network model based on the training data set, the method further includes: Calculate the mean of the position differences at all times; Calculate the standard deviation of the position differences at all times; Abnormal position differences and the joint positions and angular velocities at the same time as the abnormal position differences are eliminated based on the mean and standard deviation.

[0053] The calculation formula of the mean is as follows: , the standard deviation calculation formula is as follows , the abnormal position difference is, for example, to meet the condition By preprocessing the training data, abnormal data is removed in advance, further ensuring the accuracy of the end position compensation model.

[0054] In some embodiments, S10, obtaining a set of motion trajectories of the end of the robotic arm includes: Determine the set of position points at the end of the robotic arm according to the movable range of each joint arm; Determine boundary points in the set of location points; Determine the polygon boundary based on boundary points; Within the movable range formed by the polygonal boundary, the motion trajectory of the end of the robot arm in different motion directions is planned to obtain a motion trajectory set.

[0055] Among them, each joint arm has a limited range of motion. Therefore, the position point set of the end of the robot arm can be determined by traversing all the movable ranges of each joint arm, such as Figure 4 For example, all movable ranges of each joint arm can be traversed based on the Monte Carlo method, but it is not limited thereto.

[0056] Figure 4 The location point set shown can be a direct result of the traversal, or can be a result of further filtering the traversal results. A process of further filtering the traversal results can be: According to the position points in the traversal results, construct and The confidence level represented by the two-dimensional continuous probability density function is: , In the formula, for The mean of for The standard deviation of for The mean of for The standard deviation of for and The correlation coefficient of Keep confidence level greater than the preset value The location point, that is, retain Remove points with confidence level less than the preset value The confidence level is less than the preset value. The position point can be considered to basically not move to this area, so it is removed.

[0057] After determining the set of position points of the end of the robot arm, in order to realize the planning of the motion trajectory of the end of the robot arm in different motion directions, it is necessary to further determine the movable range corresponding to the set of position points. Specifically, the boundary points are determined in the set of position points, and then the polygonal boundary is determined according to the boundary points. At this time, the area formed by the polygonal boundary is the movable range for planning the motion trajectory. In this way, the range of motion corresponding to the set of position points can be determined more accurately. Figure 4 The range corresponding to the set of position points shown is used to plan the motion trajectory within the range, and obtain a suitable amount of sample data based on the motion trajectory collection.

[0058] In some embodiments, the process of determining boundary points in a set of location points may be: a. Take any point Find the point with the longest distance , the point is one of the boundary points; b. As the origin, Axis forward and reverse rays Scan clockwise to find the point with the smallest rotation angle , which is one of the boundary points; c. As the origin, Scan the vector direction clockwise to find the point with the smallest rotation angle , which is one of the boundary points; d. Repeat the above steps until you find the point , that is, all boundary points are collected.

[0059] In some embodiments, the process of determining the polygon boundary according to the boundary points may be: a. Curve-shaped boundaries obtained by curve fitting, such as Figure 5 As shown by the middle curve; b. Take a straight line between the first and last points (end points) of each curve of the curved boundary; c. Take the point on the curve that has the largest distance from the straight line; d. sequentially connect one of the endpoints of the curve, the point with the largest distance, and the other endpoint of the curve to obtain an approximate polyline of the curve; e. Repeat the above steps to complete the approximation of all curve parts and obtain the polygonal boundary, such as Figure 6 shown.

[0060] It should be noted that the use of polygonal boundaries can make subsequent path planning more efficient, but the boundary corresponding to the set of position points at the end of the robot arm is not limited to a polygonal boundary, and path planning can also be implemented.

[0061] In some embodiments, within the movable range formed by the polygonal boundary, the motion trajectory of the end of the robot arm in different motion directions is planned to obtain a set of motion trajectories, including: A dividing line is drawn with a concave point in the polygonal boundary as a starting point, so as to divide the movable range formed by the polygonal boundary into convex polygonal areas through the dividing line; Plan the motion trajectory perpendicular or parallel to the dividing line in each convex polygon area with the same path interval; The dividing line is rotated by a preset angle, and the motion trajectory perpendicular to the dividing line is continuously planned in each convex polygon area at the same path interval until the dividing line is rotated to the initial position; Get all the planned motion trajectories and obtain a motion trajectory set.

[0062] Among them, please refer to Figure 7 , draw the dividing line from the concave point in the polygon boundary as the starting point, for example, the concave point P is perpendicular to the downward ray of the X-axis , decompose this area into two convex polygonal areas on the left and right. Then, the parallel covering strategy is sampled to perform perpendicular coverage of the ray in the two convex polygonal areas on the left and right with the same path interval ∆d. Next, the ray Rotate ∆θ clockwise, repeat the above steps, continue to plan the motion trajectory perpendicular to the dividing line in each convex polygon area with the same path interval, and then rotate ∆θ clockwise again until Rotating to the initial position completes the full coverage of the dynamic path within the movable range formed by the polygonal boundary. Afterwards, based on the motion trajectory set, the actual position of the end of the robot arm and the joint position and angular velocity of each joint arm are sampled to obtain more comprehensive dynamic sampling data.

[0063] In some embodiments, the step of acquiring the algorithm parameters of the first online correction algorithm includes: Taking the actual position of the end of the robotic arm as the first observation value and the second position of the end of the robotic arm as the first prediction value, the algorithm parameters of the first online correction algorithm are calculated so that the error between the first observation value and the first prediction value meets the first preset condition.

[0064] Among them, the steps S10-S30 in the previous text can be used to obtain the data set for the actual position used as the first observation value and the second position used as the first prediction value, which will not be repeated here. Afterwards, select a suitable online correction algorithm, such as edge particle filtering, unscented Kalman filtering, etc., with the error between the first observation value and the first prediction value as small as possible as the optimization goal, calculate the corresponding iterative optimization parameter matrix and iterative weight parameter matrix, substitute the iterative optimization parameter matrix and iterative weight parameter matrix into the selected online correction algorithm, and obtain the first online correction algorithm. During the operation of the robotic arm, the first online correction algorithm can be used to correct the second position in real time to complete the precise update of the end position.

[0065] In some embodiments, the step of acquiring the algorithm parameters of the second online correction algorithm includes: Taking the actual position of the end of the robotic arm as the second observation value and the compensated second position of the end of the robotic arm as the second prediction value, the algorithm parameters of the second online correction algorithm are calculated so that the error between the second observation value and the second prediction value meets the second preset condition.

[0066] Among them, the steps S10-S30 in the previous text can be used to obtain a data set of the actual position used as the second observation value, and the second position obtained by using the steps S10-S30 in the previous text can be compensated by the end position compensation model to obtain a data set of the second position used as the second prediction value, which will not be repeated here. Afterwards, select a suitable online correction algorithm, such as edge particle filtering, unscented Kalman filtering, etc., with the error between the second observation value and the second prediction value as small as possible as the optimization goal, calculate the corresponding iterative optimization parameter matrix and iterative weight parameter matrix, substitute the iterative optimization parameter matrix and iterative weight parameter matrix into the selected online correction algorithm, and obtain the second online correction algorithm. During the operation of the robotic arm, the second online correction algorithm can be used to correct the second position after model compensation to complete the precise update of the end position.

[0067] The present invention discloses a method for determining the position of the end of a rigid-flexible coupling arm, which obtains the current joint position and angular velocity of each joint arm of the robot arm; based on the dynamic model of the rigid joint arm, determines the driving torque corresponding to the joint position and angular velocity of each joint arm; determines the first position of the end of the robot arm based on the joint position of each joint arm; based on the mechanical model of the flexible joint arm, determines the deflection of the end of the robot arm according to the driving torque of each joint arm; corrects the first position according to the deflection to determine the second position of the end of the robot arm. When determining the position of the end of the robot arm, the present invention combines the influence of the driving torque corresponding to the angular velocity of the joint arm on the deflection of the end of the robot arm, so that the position calculation result of the end of the robot arm is more consistent with the characteristics of the real-time motion of the robot arm, improves the solution accuracy of the position of the end of the robot arm, and better meets the real-time requirements of the end motion estimation. In addition, the dynamic position calculation value is used to train the compensation model to ensure the consistency of the real-time data and the model training data, and to ensure the effectiveness of the compensation model. An optimized segmentation full-path coverage method for the end of the robot arm is proposed, which can realize the collection of full-condition data and ensure the accuracy and versatility of the compensation model.

[0068] The present application also provides a working machine, including a controller and a mechanical arm, wherein the mechanical arm includes a plurality of joint arms, and when the controller executes a computer program, the method for determining the position of the end of the rigid-flexible coupling arm as described above is implemented. The present application also provides an electronic device, including a storage medium and a controller, wherein a computer program is stored on the storage medium, and when the computer program is executed by the controller, the method for determining the position of the end of the rigid-flexible coupling arm as described above is implemented.

[0069] The present application also provides a storage medium having a computer program stored thereon. When the computer program is executed by a processor, the method for determining the position of the end of the rigid-flexible coupling arm as described above is implemented.

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

Claims

1. A method for determining the position of the end of a rigid-flexible coupling arm, characterized in that The method comprises the following steps: Obtaining the current joint position and angular velocity of each joint arm of the robotic arm; Determine the driving torque corresponding to the joint position and the angular velocity of each joint arm based on a dynamic model of the rigid joint arm; and determine the first position of the end of the mechanical arm based on the joint position of each joint arm; Based on the mechanical model of the flexible joint arm, the deflection of the end of the mechanical arm is determined according to the driving torque of each joint arm; The first position is corrected according to the deflection to determine a second position of the end of the robot arm.

2. The method according to claim 1, characterized in that The mechanical model based on the flexible joint arm frame determines the deflection of the end of the mechanical arm according to the driving torque of each joint arm frame, including: Determine the external force on each joint arm frame according to the driving torque of each joint arm frame; Based on the mechanical model of the flexible joint arm, the deflection of the end of each joint arm is determined according to the external force applied to each joint arm; The deflection of the end of the robot arm is determined according to the deflection of the end of each joint arm frame.

3. The method according to claim 2, characterized in that The mechanical model based on the flexible joint arm frame determines the deflection of the end of each joint arm frame according to the external force applied to each joint arm frame, including: Based on the mechanical model of the flexible joint arm, determine the first deflection of the cross section of the corresponding joint arm caused by the bending moment of the equivalent force of the rear end arm of each joint arm, the second deflection of the cross section of the corresponding joint arm caused by the external force on each joint arm, and the third deflection of the cross section of the corresponding joint arm caused by the self-weight of each joint arm; The deflections of the ends of the respective articulated arms are determined respectively according to the first deflection, the second deflection and the third deflection of the respective articulated arms.

4. The method according to any one of claims 1 to 3, characterized in that After the first position is corrected according to the deflection to determine the second position of the end of the robotic arm, the method further includes one of the following: Compensating the second position of the end of the robotic arm based on an end position compensation model, wherein the end position compensation model is trained based on the second position and actual position of the end of the robotic arm at different times; Correcting the second position of the end of the robotic arm based on a first online correction algorithm; The second position of the end of the robot arm is compensated based on the end position compensation model, and the compensated second position is corrected based on a second online correction algorithm.

5. The method according to claim 4, characterized in that The step of training the end position compensation model comprises: Obtaining a set of motion trajectories of the end of the robotic arm; Controlling the end of the robotic arm to move at different speeds based on the motion trajectory set, and collecting the actual position of the end of the robotic arm at different times and the joint positions and angular velocities of each joint arm frame at different times; Determine the second position of the end of the robotic arm at different times according to the joint positions and angular velocities of the joint arms at different times, and determine the position difference between the second position and the actual position at the same time; Generate a training data set, each set of training data in the training data set includes the joint positions and angular velocities of each joint arm frame corresponding to the same moment, and the position difference; A preset neural network model is trained based on the training data set to obtain the end position compensation model, wherein the input of the end position compensation model is the current joint position and angular velocity of each joint arm, and the output is a compensation value for compensating for the second position of the end of the robotic arm, and the compensation value is used to characterize the position difference between the current second position of the end of the robotic arm and the actual position.

6. The method according to claim 5, characterized in that The step of obtaining a set of motion trajectories of the end of the robotic arm comprises: Determining a set of position points of the end of the robotic arm according to the movable range of each joint arm frame; Determining boundary points in the set of position points; Determine the polygon boundary according to the boundary points; Within the movable range formed by the polygonal boundary, the motion trajectories of the end of the robot arm in different motion directions are planned to obtain the motion trajectory set.

7. The method according to claim 6, characterized in that The planning of the motion trajectory of the end of the robot arm in different motion directions within the movable range formed by the polygonal boundary to obtain the motion trajectory set includes: Drawing a dividing line with a concave point in the polygonal boundary as a starting point, so as to divide the movable range formed by the polygonal boundary into convex polygonal areas through the dividing line; Planning a motion trajectory perpendicular or parallel to the dividing line in each of the convex polygonal regions at the same path interval; The dividing line is rotated by a preset angle, and a motion trajectory perpendicular to the dividing line is continuously planned in each of the convex polygonal regions at the same path interval until the dividing line is rotated to an initial position; All planned motion trajectories are acquired to obtain the motion trajectory set.

8. The method according to claim 4, characterized in that The step of acquiring the algorithm parameters of the first online correction algorithm comprises: Taking the actual position of the end of the robotic arm as a first observation value and the second position of the end of the robotic arm as a first prediction value, calculating algorithm parameters of the first online correction algorithm so that an error between the first observation value and the first prediction value meets a first preset condition; And / or, the step of acquiring the algorithm parameters of the second online correction algorithm includes: Taking the actual position of the end of the robotic arm as the second observation value and the compensated second position of the end of the robotic arm as the second prediction value, the algorithm parameters of the second online correction algorithm are calculated so that the error between the second observation value and the second prediction value meets the second preset condition.

9. A working machine, characterized in that: The invention comprises a controller and a mechanical arm, wherein the mechanical arm comprises a plurality of joint arms, and when the controller executes a computer program, the method for determining the position of the end of a rigid-flexible coupling arm as claimed in any one of claims 1 to 8 is implemented.

10. An electronic device, characterized in that: The invention comprises a storage medium and a controller, wherein a computer program is stored on the storage medium, and when the computer program is executed by the controller, the method for determining the position of the end of a rigid-flexible coupling arm as claimed in any one of claims 1 to 8 is implemented.

11. A storage medium, characterized in that: The storage medium stores a computer program, which, when executed by a processor, implements the method for determining the position of the end of a rigid-flexible coupling arm according to any one of claims 1 to 8.

Images