CN109822571B - Control method, device and equipment for mechanical arm of assembling machine - Google Patents
Control method, device and equipment for mechanical arm of assembling machine Download PDFInfo
- Publication number
- CN109822571B CN109822571B CN201910119745.2A CN201910119745A CN109822571B CN 109822571 B CN109822571 B CN 109822571B CN 201910119745 A CN201910119745 A CN 201910119745A CN 109822571 B CN109822571 B CN 109822571B
- Authority
- CN
- China
- Prior art keywords
- joint
- joint angle
- angle
- iterative
- iteration
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 51
- 238000004364 calculation method Methods 0.000 claims abstract description 29
- 239000013598 vector Substances 0.000 claims description 78
- 239000011159 matrix material Substances 0.000 claims description 56
- 238000004422 calculation algorithm Methods 0.000 claims description 25
- 238000000354 decomposition reaction Methods 0.000 claims description 21
- 238000012545 processing Methods 0.000 claims description 19
- 230000009466 transformation Effects 0.000 claims description 13
- 238000010276 construction Methods 0.000 claims description 12
- 230000009286 beneficial effect Effects 0.000 abstract description 4
- 230000008569 process Effects 0.000 description 10
- 230000008859 change Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 238000004458 analytical method Methods 0.000 description 2
- 238000012804 iterative process Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000012935 Averaging Methods 0.000 description 1
- OAICVXFJPJFONN-UHFFFAOYSA-N Phosphorus Chemical compound [P] OAICVXFJPJFONN-UHFFFAOYSA-N 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 239000012636 effector Substances 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000011426 transformation method Methods 0.000 description 1
Images
Landscapes
- Numerical Control (AREA)
- Manipulator (AREA)
Abstract
The application discloses a method for controlling a mechanical arm of an assembling machine, which divides the motion space of a large-angle rotary joint of the mechanical arm of the assembling machine into regions and sets iteration initial values of the regions, so that when the iteration operation is unconverged, the iteration initial values of other regions are used for replacing the current iteration initial value to continue the iteration operation, the inverse kinematics calculation can be correctly converged, the reliability of the inverse kinematics solution is effectively improved, and the real-time performance of the operation is ensured; furthermore, the angle of the large-angle rotary joint can be obtained after iteration, and inverse kinematics solution of the large-angle rotary joint is completed, so that the mechanical arm of the assembling machine is effectively controlled, and the operation efficiency of the assembling machine is improved. The application also discloses erector arm control device and equipment, all has above-mentioned beneficial effect.
Description
Technical Field
The application relates to the field of robot kinematics, in particular to a control method of a mechanical arm of an erector, and further relates to a control device of the mechanical arm of the erector and erector equipment comprising the control device of the mechanical arm of the erector.
Background
In the shield construction process, the segment erector is an important part of the shield machine, and the segment erector has the main function of lining prefabricated segments to the inner surface of a tunnel to prevent the tunnel from collapsing. The intelligent research of the assembling machine can improve the efficiency and the safety of the segment assembling work, and the kinematic modeling of the assembling machine is a powerful means for realizing the intelligence.
The kinematics of the robot is divided into positive kinematics and inverse kinematics, wherein the positive kinematics generally adopts a homogeneous coordinate transformation method, and the inverse kinematics can be selected by two methods, namely an analytic method and a numerical solution method. The analytic method is limited by the configuration of the robot, the solving process is complex, the universality is not realized, and the application is inconvenient. The numerical solution is usually a Newton iteration method, has better local quick convergence, and can obtain the solution with required precision by fewer iteration times. However, because the mechanical arm of the erector has a joint capable of rotating 360 degrees, when the joint needs to rotate by a large angle to reach the target pose, the inverse kinematics algorithm based on newton iteration has the condition of non-convergence or convergence to a solution completely exceeding the achievable range, so that the reliability of inverse kinematics solution is seriously influenced, and the large-angle rotating joint is difficult to be effectively controlled.
Therefore, how to solve the inverse kinematics of the large-angle rotary joint of the mechanical arm of the erector with higher performance so as to further effectively control the mechanical arm of the erector and improve the operating efficiency of the erector is a problem to be solved urgently by technical personnel in the field.
Disclosure of Invention
The method for controlling the mechanical arm of the assembling machine can solve the inverse kinematics of a large-angle rotating joint of the mechanical arm of the assembling machine with higher performance so as to further effectively control the mechanical arm of the assembling machine and improve the operation efficiency; another object of the application is to provide a erector arm control device and apparatus, also having the above beneficial effects.
In order to solve the technical problem, the application provides a control method for a mechanical arm of an erector, and the control method for the mechanical arm of the erector comprises the following steps:
when a control instruction is received, sending the obtained initial joint angle of each joint in the mechanical arm of the erector as an iterative joint angle to a preset model;
processing each iterative joint angle through the preset model to obtain a current pose;
calling a target pose, calculating to obtain a differential motion vector according to the current pose and the target pose, and calculating a differential variable of each joint according to the differential motion vector;
judging whether each differential variable meets preset iteration precision or not;
if the differential variable does not meet the preset iteration precision, judging whether the differential motion vector is lower than a historical differential motion vector;
if the difference is lower than the historical differential motion vector, superposing each differential variable to a corresponding iteration joint angle to obtain each updated iteration joint angle;
if the historical differential motion vector is not lower than the historical differential motion vector, replacing the iterative joint angle of the target joint with a preset iterative joint angle, replacing the iterative joint angles of other joints with the initial joint angle, and obtaining updated iterative joint angles; the target joint is a joint of which the rotation angle exceeds an iteration convergence limit angle, and the preset iteration joint angle is obtained by equally dividing the maximum rotation angle of the target joint according to a preset number;
taking the updated iterative joint angle as an iterative joint angle, returning to the step of processing each iterative joint angle through the preset model to obtain the current pose, and performing iterative processing until each differential variable meets the preset iterative precision to obtain each target joint angle;
and controlling the corresponding joint to move according to each target joint angle.
Preferably, the preset model is obtained by performing positive kinematics modeling on the mechanical arm of the erector through a D-H parameter method.
Preferably, the calculating a differential variable of each joint from the differential motion vector includes:
constructing a Jacobian matrix according to the connecting rod transformation matrix of each joint in the preset model and each initial joint angle;
calculating a pseudo-inverse matrix of the Jacobian matrix through a preset decomposition algorithm;
and combining the differential motion vector, and calculating according to the pseudo-inverse matrix to obtain the differential variable.
Preferably, the preset decomposition algorithm is a singular value decomposition algorithm.
Preferably, the preset iterative joint angle comprises θ0+90°，θ0-90°，θ0+180 °; wherein, theta0Is the initial joint angle of the target joint.
For solving the technical problem, the application still provides an erector arm control device, erector arm control device includes:
the initial value sending module is used for sending the obtained initial joint angle of each joint in the mechanical arm of the assembling machine as an iterative joint angle to a preset model when a control instruction is received;
the current pose calculation module is used for processing each iterative joint angle through the preset model to obtain a current pose;
the differential variable calculation module is used for calling a target pose, calculating to obtain a differential motion vector according to the current pose and the target pose, and calculating the differential variable of each joint according to the differential motion vector;
the differential variable judging module is used for judging whether each differential variable meets preset iteration precision;
the convergence judging module is used for judging whether the differential motion vector is lower than a historical differential motion vector or not if the differential variable does not meet the preset iteration precision;
the first initial value updating module is used for superposing each differential variable to a corresponding iteration joint angle to obtain each updated iteration joint angle if the differential motion vector is lower than the historical differential motion vector;
the second initial value updating module is used for replacing the iterative joint angle of the target joint with a preset iterative joint angle and replacing the iterative joint angles of other joints with the initial joint angles to obtain updated iterative joint angles if the differential motion vector is not lower than the historical differential motion vector; the target joint is a joint of which the rotation angle exceeds an iteration convergence limit angle, and the preset iteration joint angle is obtained by equally dividing the maximum rotation angle of the target joint according to a preset number;
the iteration module is used for returning the updated iteration joint angle as an iteration joint angle to the current pose calculation module for iteration processing until each differential variable meets the preset iteration precision to obtain each target joint angle;
and the control module is used for controlling the corresponding joint to move according to each target joint angle.
Preferably, the differential variable judgment module includes:
the matrix construction unit is used for constructing a Jacobian matrix according to the connecting rod transformation matrix of each joint in the preset model and each initial joint angle;
the matrix decomposition unit is used for calculating a pseudo-inverse matrix of the Jacobian matrix through a preset decomposition algorithm;
and the differential variable calculation unit is used for combining the differential motion vector and calculating and obtaining the differential variable according to the pseudo-inverse matrix.
For solving the technical problem, the application also provides erector arm control equipment, the equipment includes the erector body to and above-mentioned arbitrary erector arm controlling means.
The method for controlling the mechanical arm of the assembling machine comprises the steps that when a control instruction is received, an obtained initial joint angle of each joint in the mechanical arm of the assembling machine is used as an iteration joint angle and sent to a preset model; processing each iterative joint angle through the preset model to obtain a current pose; calling a target pose, calculating to obtain a differential motion vector according to the current pose and the target pose, and calculating a differential variable of each joint according to the differential motion vector; judging whether each differential variable meets preset iteration precision or not; if the differential variable does not meet the preset iteration precision, judging whether the differential motion vector is lower than a historical differential motion vector; if the difference is lower than the historical differential motion vector, superposing each differential variable to a corresponding iteration joint angle to obtain each updated iteration joint angle; if the historical differential motion vector is not lower than the historical differential motion vector, replacing the iterative joint angle of the target joint with a preset iterative joint angle, replacing the iterative joint angles of other joints with the initial joint angle, and obtaining updated iterative joint angles; the target joint is a joint of which the rotation angle exceeds an iteration convergence limit angle, and the preset iteration joint angle is obtained by equally dividing the maximum rotation angle of the target joint according to a preset number; taking the updated iterative joint angle as an iterative joint angle, returning to the step of processing each iterative joint angle through the preset model to obtain the current pose, and performing iterative processing until each differential variable meets the preset iterative precision to obtain each target joint angle; and controlling the corresponding joint to move according to each target joint angle.
Therefore, according to the control method of the mechanical arm of the assembling machine, the large-angle rotary joint of the mechanical arm of the assembling machine, namely the motion space of the target joint, is subjected to region division, and the iteration initial value of each region, namely the preset iteration joint angle, is set, so that when the iteration operation is not converged, the iteration initial value of other regions is used for replacing the current iteration initial value to continue the iteration operation, the inverse kinematics calculation can be correctly converged, the reliability of the inverse kinematics solution is effectively improved, and the real-time performance of the operation is ensured; furthermore, the angle of the large-angle rotary joint can be obtained after iteration, and inverse kinematics solution of the large-angle rotary joint is completed, so that the mechanical arm of the assembling machine is effectively controlled, and the operation efficiency of the assembling machine is improved.
The application provides a erector arm controlling means and equipment all has above-mentioned beneficial effect, no longer gives unnecessary details here.
Drawings
In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings needed to be used in the description of the embodiments or the prior art will be briefly introduced below, it is obvious that the drawings in the following description are only embodiments of the present application, and for those skilled in the art, other drawings can be obtained according to the provided drawings without creative efforts.
FIG. 1 is a schematic flow chart of a method for controlling a manipulator of an erector provided by the present application;
FIG. 2 is a schematic diagram illustrating a division of a large angular rotation space provided in the present application;
fig. 3 is a schematic structural diagram of a manipulator control device of the erector provided by the present application.
Detailed Description
The core of the application is to provide the control method of the mechanical arm of the assembling machine, the control method of the mechanical arm of the assembling machine can solve the inverse kinematics of the large-angle rotating joint of the mechanical arm of the assembling machine with higher performance so as to further effectively control the mechanical arm of the assembling machine and improve the operation efficiency of the assembling machine; another core of this application is to provide an erector arm control device and equipment, also has above-mentioned beneficial effect.
In order to make the objects, technical solutions and advantages of the embodiments of the present application clearer, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application, and it is obvious that the described embodiments are some embodiments of the present application, but not all embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present application.
Referring to fig. 1, fig. 1 is a schematic flow chart of a method for controlling a robot arm of a erector according to the present application, where the method for controlling the robot arm of the erector may include:
s101: when a control instruction is received, sending the obtained initial joint angle of each joint in the mechanical arm of the erector as an iterative joint angle to a preset model;
when the erector needs to be started for construction, a constructor can send a control instruction to the processor through the corresponding terminal, and the processor controls the mechanical arm of the erector according to the control instruction. Specifically, the step aims to input the initial joint angle of each joint on the mechanical arm of the erector as an iterative joint angle to a preset model for processing based on the received control instruction.
The preset model is obtained by performing positive kinematics modeling on the mechanical arm of the assembling machine through a D-H parameter method.
The embodiment of the application provides a specific construction method of a preset model, namely the construction method is realized based on a D-H parameter method. Specifically, the D-H parametric method is used for fixing a coordinate system on each connecting rod of the robot, and then the spatial relationship of two adjacent connecting rods is described through a 4 x 4 homogeneous transformation matrix, so that the pose of the end effector relative to a base coordinate system can be finally deduced through sequential transformation, and a kinematic equation of the robot is established. Then, in the technical solution, a coordinate system may be established on each link of the robot arm, a transformation matrix between two adjacent links is obtained by calculation according to the initial joint angles and the basic parameters of the robot arm, and further, a link change matrix from the terminal coordinate system of the robot arm to the base coordinate system may be derived by sequential transformation, thereby completing the positive kinematics modeling of the robot arm of the erector and obtaining the preset model.
S102: processing each iterative joint angle through a preset model to obtain a current pose;
on the basis of S101, the calculation of the current pose of the mechanical arm of the erector is realized based on the iterative joint angles of the joints, specifically, after the iterative joint angles are input into a preset model, the preset model automatically processes the iterative joint angles according to the self operation rule to obtain the current pose.
S103: calling a target pose, calculating according to the current pose and the target pose to obtain a differential motion vector, and calculating a differential variable of each joint according to the differential motion vector;
the method comprises the steps of calculating differential variables based on the current pose and the target pose of the mechanical arm, and calling the target pose after the current pose of the mechanical arm is obtained based on S102, wherein the target pose can be obtained through professional research calculation and is pre-stored in a storage space and can be directly called by a processor; further, a differential motion matrix is calculated according to the current pose and the target pose, so that a differential motion vector is obtained, and differential variables of all joints of the mechanical arm are obtained based on differential motion vector calculation.
The calculating the differential variable of each joint according to the differential motion vector may include: constructing a Jacobian matrix according to the connecting rod transformation matrix of each joint in the preset model and each initial joint angle; calculating a pseudo-inverse matrix of the Jacobian matrix through a preset decomposition algorithm; and combining the differential motion vector, and calculating according to the pseudo-inverse matrix to obtain a differential variable.
For the above calculation of the differential variable based on the differential motion vector, the present application provides a more specific implementation method, namely, based on the jacobian matrix. The jacobian matrix is analogous to the derivative of a multivariate function, which embodies an optimal linear approximation of a differentiable equation to a given point. Specifically, firstly, a jacobian matrix can be constructed according to the link transformation matrix of each joint in the preset model and the initial joint angle of each joint, and then the connection transformation matrix and the initial joint angle are decomposed by a preset decomposition algorithm to obtain a corresponding pseudo-inverse matrix, so that the differential variable of each joint is calculated and obtained based on the pseudo-inverse matrix and the differential motion vector.
The preset decomposition algorithm may be a singular value decomposition algorithm.
For the preset Decomposition algorithm, a Singular Value Decomposition (SVD) algorithm may be used, and the algorithm is simple and practical. Of course, the algorithm is only one implementation manner provided by the present application, and is not unique, and may also be other matrix decomposition algorithms, which can realize the decomposition of the jacobian matrix.
S104: judging whether each differential variable meets a preset standard or not; if not, executing S105, and if so, executing S109;
in order to ensure the construction safety of the erector, certain precision requirements are provided for each joint of the mechanical arm of the erector, the step aims to realize precision judgment of each joint, and the judgment process can be realized based on differential variables, and specifically, whether the differential variables of each erector meet the preset iteration precision or not can be judged. If the preset iteration precision is met, S109 can be executed, the initial joint angle is used as the following target joint angle, and each joint is operated according to the corresponding target joint angle to carry out construction; if the differential variable does not meet the preset iteration precision, it indicates that the construction cannot be continued on the basis, and at this time, S105 is executed to enter a subsequent iteration process to obtain a target joint angle meeting the preset iteration precision.
The specific values of the preset iteration precision can be set by technicians according to actual requirements, and the method is not limited in this application. In addition, the preset iteration precision can be a unified standard relative to each joint angle, namely all the joint angles correspond to the same standard, and can also correspond to each joint angle one by one, namely each joint angle has the corresponding precision standard, so that the accuracy is further improved, and the construction safety is ensured.
S105: judging whether the differential motion vector is lower than the historical differential motion vector; if yes, executing S106, otherwise executing S107;
and when the differential variable of each joint angle is determined to be incapable of meeting the preset iteration precision, starting to enter an iteration operation process. The step aims to judge whether the iterative algorithm is converged by judging whether the differential motion vector is lower than the historical differential motion vector, wherein the historical differential motion vector is the differential motion vector obtained in the last iterative process of the current iterative process, and when the differential motion vector is not lower than the historical differential motion vector, the actual solution cannot be obtained through iteration. Therefore, for the convergence condition and the non-convergence condition of the iterative algorithm, the initial joint angle of each joint needs to be updated in different ways to complete the iterative operation, and for the specific process of updating, please refer to the following description, S106 is executed when the differential motion vector is lower than the historical differential motion vector, and S107 is executed when the differential motion vector is not lower than the differential motion vector.
S106: superposing each differential variable to the corresponding iterative joint angle to obtain each updated iterative joint angle;
when the differential motion vector is determined to be lower than the historical differential motion vector, each differential variable can be added to the corresponding initial joint angle to update the iterative joint angle, each updated iterative joint angle is obtained, and further iterative operation can be started according to each updated iterative joint angle.
S107: replacing the iterative joint angle of the target joint with a preset iterative joint angle, replacing the iterative joint angles of other joints with initial joint angles, and obtaining updated iterative joint angles; the target joint is a joint of which the rotation angle exceeds an iteration convergence limit angle, and the preset iteration joint angle is obtained by equally dividing the maximum rotation angle of the target joint according to a preset number;
when the differential motion vector is determined not to be lower than the historical differential motion vector, the joint of which the rotation angle exceeds the iteration convergence limit angle, namely a large-angle rotation joint, namely the initial joint angle of the target joint, is replaced by a preset iteration joint angle, the preset iteration joint angle is obtained by equally dividing the maximum rotation space of the target joint according to a preset number, and the specific determination process is as follows: according to the initial joint angle of the target joint, dividing the maximum rotation angle of the target joint into 4 parts, 6 parts, 8 parts and the like according to a preset number, wherein the number of the obtained preset iterative joint angles is 3, 5 and 7 correspondingly; further, the value of each averaging point is taken as the preset iterative joint angle. Of course, for the replacement sequence of each preset iterative joint angle, the replacement is performed according to the sequence of the iterative joint angles from the near to the far from the target joint. In addition, for other joints than the target joint, the corresponding iterative joint angle may be replaced with the initial joint angle, that is, the initial joint angle acquired in S101. Therefore, each updated iterative joint angle can be obtained, and iterative operation is started according to each updated iterative joint angle.
In an embodiment provided by the present application, the preset iterative joint angle may include θ0+90°，θ0-90°，θ0+180 °; wherein, theta0Is the initial joint angle of the target joint.
The embodiment of the present application provides a more specific method for setting a preset iterative joint angle, please refer to fig. 2, and fig. 2 is a schematic diagram illustrating a division of a large-angle rotation space provided in the present application. Specifically, the 360-degree rotatable angle is divided into 4 parts, and the initial joint angle of the target joint is assumed to be theta0Then the predetermined iterative joint angle may include θ0+90°，θ0-90°，θ0+180 °. Thus, during the iteration process, the algorithm does not accept every time it is iteratedWhen converging, the theta can be sequentially adjusted0Substitution to theta0+90 °; will theta0+90 ° by θ0-90 °, will θ0-90 ° to θ0+180 ° to effectively guarantee convergence of the iterative algorithm, thereby calculating the true solution.
S108: taking the updated iterative joint angle as an iterative joint angle, returning to S102 for iterative processing until each differential variable meets preset iterative precision;
after the updating of the iterative joint angle is completed and the corresponding updated iterative joint angle is obtained, the updated iterative joint angle can be used as the iterative joint angle again to return to the step S102 for iterative processing until the differential variable of each joint meets the preset iterative precision, the iterative loop is ended, and the step S109 is executed.
S109: and when the differential variable meets the preset iteration precision, obtaining each target joint angle, and controlling the corresponding joint to move according to each target joint angle.
Specifically, when the differential variables of the joints satisfy the iteration accuracy, the joint angle of each joint obtained after the iteration is used as the target joint angle, and the joints of the manipulator of the erector are controlled according to the target joint angles so as to move according to the corresponding target joint angles, thereby completing the construction of the erector.
According to the control method for the mechanical arm of the assembling machine, the large-angle rotary joint of the mechanical arm of the assembling machine, namely the motion space of the target joint, is subjected to region division, and the iteration initial value of each region, namely the preset iteration joint angle, is set, so that when the iteration operation is unconverged, the iteration initial value of other regions is used for replacing the current iteration initial value to continue the iteration operation, the inverse kinematics calculation can be correctly converged, the reliability of the inverse kinematics solution is effectively improved, and the real-time performance of the operation is ensured; furthermore, the angle of the large-angle rotary joint can be obtained after iteration, and inverse kinematics solution of the large-angle rotary joint is completed, so that the mechanical arm of the assembling machine is effectively controlled, and the operation efficiency of the assembling machine is improved.
On the basis of the various embodiments, the application provides a more specific erector manipulator control method.
Firstly, carrying out positive kinematics modeling on the mechanical arm of the assembling machine by a D-H parameter method to obtain the preset model, wherein in the D-H parameter, ai-1The length of the connecting rod, namely the length of a common perpendicular line between the joint axis i-1 and the joint axis i; alpha is alphai-1The angle of the connecting rod is the included angle between the joint axis i-1 and the joint axis i; diThe offset distance of the connecting rods is the distance in the direction of the common axis of two adjacent connecting rods; thetaiThe initial joint angle of the target joint in the above embodiment is shown, i.e. the angle at which two adjacent links rotate about a common axis.
Thus, the transformation matrix of the ith coordinate system relative to the ith-1 coordinate system can be calculated by the D-H parameter method
In the modeling process, the mechanical arm is preset to be a seven-degree-of-freedom mechanical arm, and then a connecting rod change matrix from the tail end coordinate system of the mechanical arm to the base coordinate system is as follows:
the joint 2 of the mechanical arm of the known erector is a large-angle rotary joint, namely a target joint, and the change range of the joint angle theta is as follows: +197 ° -197 °, assuming an initial joint angle θ0Will theta0Substituting the formula into the formula to obtain the current pose T of the mechanical armcur；
In addition, the target pose T of the mechanical arm is knowntagComprises the following steps:
wherein, TtagEach element in the series is a constant;
further, according to the target pose TtagAnd current pose TcurCalculating a differential motion matrix Δ:
wherein I is an identity matrix;
thus, a differential motion vector D (theta) can be calculated from the corresponding elements in deltak)：
D(θk)＝[x y z dx dy dz]T；
Further, a Jacobian matrix J (theta) is constructed according to the iterative joint angles of all jointsk) And d, setting dq as a differential variable of each joint angle, so as to establish an iterative equation of a Newton iterative method:
J(θk)·dq＝D(θk)；
solving the Jacobian matrix J (theta) through an SVD (singular value decomposition) algorithmk) Pseudo-inverse matrix of (J + (θ)k) And then:
dq＝J+(θk)·D(θk)；
thus, the current differential variable dq of each joint angle is obtained.
Further, dq is compared with the set iteration precision, namely the preset standard, and the condition | | | | dq | calculation of the Y phosphor is judged∞If not more than the set value. If yes, exiting the iteration loop; if the joint angle does not satisfy the condition, the differential variable dq of each joint is superposed on the corresponding iteration joint angle, and the circulation is continued.
Further, when the number of iterative computations is greater than or equal to two, the differential motion vector D (θ) currently computed can be calculatedk) And the differential motion vector D (theta) of the previous cyclek-1) Respectively taking 1 norm and judging condition | | | D (theta)k)||1≤||D(θk-1)||1Whether D (theta) is satisfied or not is judgedk) Whether or not to converge. If yes, continuing the next iterative loop calculation; if not, then theta will be added0+90 ° alternative θ0And othersAnd replacing all joint angles of the joints with the initial joint angles, and restarting the inverse kinematics iterative calculation.
Judging the condition | D (theta) again after the newly started iterative computation times are more than or equal to two timesk)||1≤||D(θk-1)||1Whether or not this is true. If yes, continuing the next iterative loop calculation; if not, then theta will be added0-90 ° instead of θ0+90 °, the joint angles of the other joints are all replaced with the initial joint angles, and the inverse kinematics iterative calculation is restarted.
After the newly started iterative loop operation is more than or equal to two times, the condition | | | D (theta) is continuously judgedk)||1≤||D(θk-1)||1Whether or not this is true. If yes, continuing the next iterative loop calculation; if not, then theta will be added0+180 ° replacement of θ0-90 °, the joint angles of the other joints are all replaced by the initial joint angle, and the inverse kinematics iterative calculation is restarted.
Therefore, for any target joint angle of the large-angle rotary joint of the mechanical arm of the assembling machine, the inverse kinematics iterative operation can be converged by at most three times of iterative initial value change, so that the correct angle position of the target joint angle of the large-angle rotary joint and the target joint angles of other joints are obtained.
Since the rotational motion range of the large-angle rotary joint of the robot arm of the erector is from-197 ° to 197 °, the target joint angle θ obtained by the inverse kinematics calculationtagOnly the position of the target joint angle can be determined, and the actual target joint angle of the joint still needs to be further calculated and determined, so that the erector is guided to move in the direction meeting the actual condition to reach the target pose.
Specifically, when-197 ° < θtagWhen the condition of less than 197 degrees is met, thetatagThe actual target joint angle of the joint is obtained; when theta istag> 197 deg. thetatag360 ° is the actual target joint angle for that joint; when theta istagTheta at < -197 DEGtag+360 ° is the actual target joint angle for that joint.
The control method for the mechanical arm of the assembling machine, provided by the embodiment of the application, can solve the inverse kinematics of the large-angle rotary joint of the mechanical arm of the assembling machine with higher performance so as to further effectively control the mechanical arm of the assembling machine and improve the operation efficiency of the assembling machine.
Referring to fig. 3, fig. 3 is a schematic structural diagram of a robot control device of a erector provided in the present application, where the robot control device of the erector may include:
the initial value sending module 10 is configured to send the obtained initial joint angle of each joint in the mechanical arm of the erector as an iterative joint angle to a preset model when receiving a control instruction;
the current pose calculation module 20 is used for processing each iterative joint angle through a preset model to obtain a current pose;
the differential variable calculation module 30 is configured to retrieve a target pose, calculate to obtain a differential motion vector according to the current pose and the target pose, and calculate a differential variable of each joint according to the differential motion vector;
a differential variable judgment module 40, configured to judge whether each differential variable satisfies a preset iteration precision;
a convergence judging module 50, configured to, if the differential variable does not satisfy the preset iteration precision, judge whether the differential motion vector is lower than the historical differential motion vector;
a first initial value updating module 60, configured to, if the differential motion vector is lower than the historical differential motion vector, superimpose each differential variable onto a corresponding iterative joint angle to obtain each updated iterative joint angle;
a second initial value updating module 70, configured to replace the iterative joint angle of the target joint with a preset iterative joint angle and replace the iterative joint angles of other joints with initial joint angles if the differential motion vector is not lower than the historical differential motion vector, so as to obtain updated iterative joint angles; the target joint is a joint of which the rotation angle exceeds an iteration convergence limit angle, and the preset iteration joint angle is obtained by equally dividing the maximum rotation angle of the target joint according to a preset number;
the iteration module 80 is configured to use the updated iteration joint angle as an iteration joint angle, return to the current pose calculation module 20, and perform iteration processing until each differential variable meets preset iteration precision, so as to obtain each target joint angle;
and the control module 90 is used for controlling the corresponding joint to move according to each target joint angle.
As a preferred embodiment, the differential variable calculation module 30 may include:
the matrix construction unit is used for constructing a Jacobian matrix according to the connecting rod transformation matrix of each joint in the preset model and each initial joint angle;
the matrix decomposition unit is used for calculating a pseudo-inverse matrix of the Jacobian matrix through a preset decomposition algorithm;
and the differential variable calculating unit is used for combining the differential motion vector and obtaining a differential variable according to the pseudo-inverse matrix calculation.
For the specific content of the device provided in the present application, the detailed description is omitted here.
The application also provides an erector mechanical arm control device, which can comprise an erector body and any one of the erector mechanical arm control devices, wherein the installation position of the erector mechanical arm control device on the erector does not influence the implementation of the technical scheme. For the specific content of the assembling machine provided by the present application, the detailed description is omitted here.
The embodiments are described in a progressive manner in the specification, each embodiment focuses on differences from other embodiments, and the same and similar parts among the embodiments are referred to each other. The device disclosed by the embodiment corresponds to the method disclosed by the embodiment, so that the description is simple, and the relevant points can be referred to the method part for description.
Those of skill would further appreciate that the various illustrative elements and algorithm steps described in connection with the embodiments disclosed herein may be implemented as electronic hardware, computer software, or combinations of both, and that the various illustrative components and steps have been described above generally in terms of their functionality in order to clearly illustrate this interchangeability of hardware and software. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the implementation. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present application.
The detailed description is given above to the method, device and equipment for controlling the mechanical arm of the erector. The principles and embodiments of the present application are explained herein using specific examples, which are provided only to help understand the method and the core idea of the present application. It should be noted that, for those skilled in the art, it is possible to make several improvements and modifications to the present application without departing from the principle of the present application, and these improvements and modifications also fall into the elements of the protection scope of the claims of the present application.
Claims (8)
1. A control method for a mechanical arm of an erector is characterized by comprising the following steps:
when a control instruction is received, sending the obtained initial joint angle of each joint in the mechanical arm of the erector as an iterative joint angle to a preset model;
processing each iterative joint angle through the preset model to obtain a current pose;
calling a target pose, calculating to obtain a differential motion vector according to the current pose and the target pose, and calculating a differential variable of each joint according to the differential motion vector;
judging whether each differential variable meets preset iteration precision or not;
if the differential variable does not meet the preset iteration precision, judging whether the differential motion vector is lower than the previous differential motion vector;
if the difference is lower than the previous differential motion vector, superposing each differential variable to a corresponding iteration joint angle to obtain each updated iteration joint angle;
if the difference is not lower than the last differential motion vector, replacing the iterative joint angle of the target joint with a preset iterative joint angle, and replacing the iterative joint angles of other joints with the initial joint angle to obtain updated iterative joint angles; the target joint is a joint of which the rotation angle exceeds an iteration convergence limit angle, and the preset iteration joint angle is obtained by equally dividing the maximum rotation angle of the target joint according to a preset number;
taking the updated iterative joint angle as an iterative joint angle, returning to the step of processing each iterative joint angle through the preset model to obtain the current pose, and performing iterative processing until each differential variable meets the preset iterative precision to obtain each target joint angle;
and controlling the corresponding joint to move according to each target joint angle.
2. The erector arm control method of claim 1 wherein said preset model is obtained by positive kinematic modeling of said erector arm by D-H parametric method.
3. The erector robotic arm control method of claim 1, wherein said calculating a differential variable for each of said joints from said differential motion vectors comprises:
constructing a Jacobian matrix according to the connecting rod transformation matrix of each joint in the preset model and each initial joint angle;
calculating a pseudo-inverse matrix of the Jacobian matrix through a preset decomposition algorithm;
and combining the differential motion vector, and calculating according to the pseudo-inverse matrix to obtain the differential variable.
4. The erector robotic arm control method of claim 3 wherein said predetermined decomposition algorithm is a singular value decomposition algorithm.
5. The erector robotic arm control method of any one of claims 1 to 4 wherein said predetermined iterative joint angle comprises θ0+90°，θ0-90°，θ0+180 °; wherein，θ0Is the initial joint angle of the target joint.
6. The utility model provides an erector arm control device which characterized in that includes:
the initial value sending module is used for sending the obtained initial joint angle of each joint in the mechanical arm of the assembling machine as an iterative joint angle to a preset model when a control instruction is received;
the current pose calculation module is used for processing each iterative joint angle through the preset model to obtain a current pose;
the differential variable calculation module is used for calling a target pose, calculating to obtain a differential motion vector according to the current pose and the target pose, and calculating the differential variable of each joint according to the differential motion vector;
the differential variable judging module is used for judging whether each differential variable meets preset iteration precision;
a convergence judging module, configured to judge whether the differential motion vector is lower than a previous differential motion vector if the differential variable does not satisfy the preset iteration precision;
the first initial value updating module is used for superposing each differential variable to a corresponding iteration joint angle to obtain each updated iteration joint angle if the differential motion vector is lower than the last differential motion vector;
the second initial value updating module is used for replacing the iterative joint angle of the target joint with a preset iterative joint angle and replacing the iterative joint angles of other joints with the initial joint angles to obtain updated iterative joint angles if the differential motion vector is not lower than the last differential motion vector; the target joint is a joint of which the rotation angle exceeds an iteration convergence limit angle, and the preset iteration joint angle is obtained by equally dividing the maximum rotation angle of the target joint according to a preset number;
the iteration module is used for returning the updated iteration joint angle as an iteration joint angle to the current pose calculation module for iteration processing until each differential variable meets the preset iteration precision to obtain each target joint angle;
and the control module is used for controlling the corresponding joint to move according to each target joint angle.
7. The robot arm control device according to claim 6, wherein the differential variable judging module comprises:
the matrix construction unit is used for constructing a Jacobian matrix according to the connecting rod transformation matrix of each joint in the preset model and each initial joint angle;
the matrix decomposition unit is used for calculating a pseudo-inverse matrix of the Jacobian matrix through a preset decomposition algorithm;
and the differential variable calculation unit is used for combining the differential motion vector and calculating and obtaining the differential variable according to the pseudo-inverse matrix.
8. A erector robot arm control apparatus comprising an erector body, and a erector robot arm control device as defined in claim 6 or 7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910119745.2A CN109822571B (en) | 2019-02-18 | 2019-02-18 | Control method, device and equipment for mechanical arm of assembling machine |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910119745.2A CN109822571B (en) | 2019-02-18 | 2019-02-18 | Control method, device and equipment for mechanical arm of assembling machine |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109822571A CN109822571A (en) | 2019-05-31 |
| CN109822571B true CN109822571B (en) | 2020-12-08 |
Family
ID=66863773
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910119745.2A Active CN109822571B (en) | 2019-02-18 | 2019-02-18 | Control method, device and equipment for mechanical arm of assembling machine |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109822571B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110948482A (en) * | 2019-11-06 | 2020-04-03 | 江苏信息职业技术学院 | Redundant robot trajectory planning method |
| CN117207200B (en) * | 2023-11-09 | 2024-07-23 | 湖南视比特机器人有限公司 | Method and device for generating working space of mechanical arm and computer equipment |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US3920972A (en) * | 1974-07-16 | 1975-11-18 | Cincinnati Milacron Inc | Method and apparatus for programming a computer operated robot arm |
| CN102880801B (en) * | 2012-09-28 | 2015-07-08 | 东南大学 | Analysis method for motion process of over-constraint mechanism |
| CN106844985B (en) * | 2017-02-06 | 2019-08-16 | 中国科学院计算技术研究所 | A kind of fast solution method and system of high-freedom degree Robotic inverse kinematics |
| CN107685330B (en) * | 2017-10-18 | 2018-12-18 | 佛山华数机器人有限公司 | A kind of Inverse Kinematics Solution method for solving of six degree of freedom wrist bias series robot |
| CN108761399B (en) * | 2018-06-01 | 2020-09-25 | 中国人民解放军战略支援部队信息工程大学 | Passive radar target positioning method and device |
-
2019
- 2019-02-18 CN CN201910119745.2A patent/CN109822571B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN109822571A (en) | 2019-05-31 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106406277B (en) | Robot kinematics' parameter error Optimization Compensation method and device | |
| CN109822571B (en) | Control method, device and equipment for mechanical arm of assembling machine | |
| CN106737689A (en) | Super redundant mechanical arm based on mode function mixes Converse solved method and system | |
| CN110253574B (en) | Multi-task mechanical arm pose detection and error compensation method | |
| CN105856231B (en) | A kind of motion control method of particular configuration six-shaft industrial robot | |
| EP3511127B1 (en) | Controller for robot and inverse transform method for robot | |
| He et al. | A tolerance constrained G2 continuous path smoothing and interpolation method for industrial SCARA robots | |
| CN110039548B (en) | Control method, device and equipment for assembling machine | |
| KR102405096B1 (en) | Method for commanding an automated work cell | |
| US7248012B2 (en) | Teaching data preparing method for articulated robot | |
| CN111993414A (en) | Mechanical arm multi-joint linkage control method | |
| KR19980086498A (en) | A locus control device and a locus control method of a multi-degree-of-freedom scalar type robot in a plane, and a locus control program of a multi-degree-of-freedom scalar type robot in a plane | |
| CN109366486B (en) | Flexible robot inverse kinematics solving method, system, equipment and storage medium | |
| Yang et al. | Optimal configuration for mobile robotic grinding of large complex components based on redundant parameters | |
| CN117287126A (en) | Inverse solution control method for drill boom of drill jumbo | |
| CN114055448B (en) | Rope-driven snakelike mechanical arm control method, device and equipment | |
| CN116442211A (en) | Mechanical arm control method and device and terminal equipment | |
| JP2703767B2 (en) | Robot teaching data creation method | |
| CN110587596A (en) | Multi-axis configuration device remote control method and device, terminal equipment and storage medium | |
| CN115805587A (en) | Motion analysis method and device of seven-axis robot and electronic equipment | |
| CN114734436A (en) | Robot encoder calibration method and device and robot | |
| CN112917479A (en) | Approximate pose calculation method and device of five-axis robot and storage medium | |
| CN109531573B (en) | Robot posture smooth path generation method based on sample lines | |
| Xiang et al. | An optimal trajectory control strategy for underwater welding robot | |
| CN117961889A (en) | Mechanical arm action driving method, electronic equipment and storage medium |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| CB02 | Change of applicant information |
Address after: 410100 No. 88 East 7 Line, Changsha Economic and Technological Development Zone, Changsha City, Hunan Province Applicant after: China Railway Construction Heavy Industry Co.,Ltd. Address before: 410100 No. 88 East 7 Line, Changsha Economic and Technological Development Zone, Changsha City, Hunan Province Applicant before: China Railway Construction Heavy Industry Co.,Ltd. |
|
| CB02 | Change of applicant information | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |