CN108247638B - Control method of multi-degree-of-freedom rotary mechanical arm - Google Patents

Control method of multi-degree-of-freedom rotary mechanical arm Download PDF

Info

Publication number
CN108247638B
CN108247638B CN201810089456.8A CN201810089456A CN108247638B CN 108247638 B CN108247638 B CN 108247638B CN 201810089456 A CN201810089456 A CN 201810089456A CN 108247638 B CN108247638 B CN 108247638B
Authority
CN
China
Prior art keywords
mechanical arm
steering engine
steering
mechanical
degree
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.)
Expired - Fee Related
Application number
CN201810089456.8A
Other languages
Chinese (zh)
Other versions
CN108247638A (en
Inventor
唐达
付群
王德宇
母庆冉
曾宪鹏
岳前进
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Dalian University of Technology
Original Assignee
Dalian University of Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Dalian University of Technology filed Critical Dalian University of Technology
Priority to CN201810089456.8A priority Critical patent/CN108247638B/en
Publication of CN108247638A publication Critical patent/CN108247638A/en
Application granted granted Critical
Publication of CN108247638B publication Critical patent/CN108247638B/en
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1679—Programme controls characterised by the tasks executed
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Toys (AREA)
  • Manipulator (AREA)

Abstract

The invention discloses a control method of a multi-degree-of-freedom rotary mechanical arm, which can realize that the multi-degree-of-freedom rotary mechanical arm automatically grabs an object to a given three-dimensional coordinate point and can realize linear motion of a mechanical claw of the multi-degree-of-freedom rotary mechanical arm when the direction of the mechanical claw grabs the object is required to be considered and when the direction of the mechanical claw grabs the object is not required to be considered. And according to the coordinate of the target point in the three-dimensional rectangular coordinate system, adding constraint conditions and calculating the posture angle of each limb joint of the mechanical arm, so that the mechanical claw directly reaches the target point, and the fixed-point automatic grabbing of the multi-freedom-degree rotary mechanical claw is realized. The invention mainly solves the problem of complex operation of the multi-degree-of-freedom rotary mechanical arm and simplifies the control method of the original multi-degree-of-freedom rotary mechanical arm.

Description

Control method of multi-degree-of-freedom rotary mechanical arm
Technical Field
The invention relates to a control method of a mechanical arm, in particular to a control method of a rotary mechanical arm.
Background
The problem that the cost is high, the danger is high, the personnel are in short supply and the like exist when the sea cucumbers are manually caught, and the situation that people are caught by mechanical arms instead of people becomes a necessary development trend. Most of the current industrial mechanical arms belong to the first generation of mechanical arms, namely, the mechanical arms under the control of manual open-loop control, and most of the mechanical arms can only finish single and repeated work. Due to the factors of complex seabed fishing environment, unfixed sea cucumber position, insufficient mechanical arm waterproof degree, complex grabbing operation and the like, the first-generation mechanical arm cannot be directly used for fishing sea cucumbers. In order to meet the requirements of underwater fishing operation, the structure, a control algorithm and a control system of the existing mechanical arm need to be improved.
The existing mechanical arm controlled by the rotation of the steering engine mostly takes single steering engine control as a main part, namely all the steering engines on the mechanical arm are controlled independently. For a mainstream six-degree-of-freedom rotary mechanical arm, 6 steering engines need to be controlled. The control input is more, and the operation is complicated, because when every steering wheel of adjusting all the way, the gripper all can rotate according to different radiuses with the axle of difference, need adjust a plurality of steering wheels repeatedly before reaching the target point like this, both influenced work efficiency, do not conform to people's visual control's thinking custom again, upper and lower, left and right, preceding, back straight line adjustment promptly. The multi-degree-of-freedom rotary mechanical arm on the existing assembly line mostly works in a motion group mode, and the essence is that the motion is pre-programmed into an instruction to be stored, and then the mechanical arm reads the instruction once and again to move. The method is only suitable for occasions with high repeatability and fixed target points, and is not suitable for the situations of man-machine interaction and uncertain target points in the similar sea cucumber catching operation scene.
Disclosure of Invention
The technical problem to be solved by the invention is to provide a universal control method of a multi-degree-of-freedom rotary mechanical arm, which adds constraint conditions and calculates the posture angle of each limb joint of the mechanical arm according to the coordinate of a target point in a three-dimensional rectangular coordinate system, so that a mechanical claw directly reaches the target point, thereby realizing fixed-point automatic grabbing of an object by the multi-degree-of-freedom rotary mechanical claw, namely adjusting the coordinate in a three-dimensional space of the mechanical claw at the previous moment according to a current instruction, updating the coordinate of the target point, and further realizing linear motion control.
The technical scheme of the invention is realized as follows:
the method for controlling a multi-degree-of-freedom rotary robot arm includes the following steps, step S1: adding constraint conditions to calculate the posture angle of each limb joint of the mechanical arm according to the coordinates of the target point in the three-dimensional rectangular coordinate system; step S2: determining the rotation angle of each steering engine on the mechanical arm according to the attitude angle in the step S1; step S3: determining PWM (pulse width modulation) values of all steering engines according to the rotation angle in the step S2; step S4: and (4) simultaneously sending the numerical value signals of the PWM values of the steering engines in the step (S3) to the steering engines, enabling the steering engines to rotate to the attitude angle in the step (S1), and realizing the coincidence of the center of the mechanical claw and the target point, namely realizing the operation of the mechanical claw to a given three-dimensional space coordinate point.
Preferably, the constraint conditions added in step S1 include two types, which are a first constraint condition with equal arm length of the mechanical arm and a second constraint condition with unequal arm length of the mechanical arm and no obvious regularity.
Preferably, the multi-degree-of-freedom rotary mechanical arm is provided with N paths of steering gears (N is more than or equal to 6), wherein the steering gears N-1 control the mechanical claws to open and close, the steering gears N-2 control the mechanical claws to rotate, and the steering gears NN controls the chassis to rotate, and the rest steering engines control the mechanical arm limbs in the vertical plane to move; the coordinate of the three-dimensional rectangular coordinate system takes the center of a steering engine N- (N-1) rotating shaft as a coordinate origin O, and an XOY plane of the coordinate system is always parallel to the plane of the mechanical arm base; steering engine N- (N-2) and steering engine N- (N-1) rotating shaft distance OD1Is L1Steering engine N- (N-3) and steering engine N- (N-2) rotating shaft distance D1D2Is L2By parity of reasoning, the distance D from the center of the mechanical claw to the N-3 rotating shaft of the steering enginenDn-1Is Ln。
Preferably, the arm length of the mechanical arm is equal under the first constraint condition, i.e. Li＝Ln+1-i(i is not less than 1 and not more than n-1, i is an integer), and when n is an odd number, at ODnTaking n-1 points H on the line segment1、H2、……Hn-1To make
Figure BDA0001563295610000021
Figure BDA0001563295610000022
In Hi(i is more than or equal to 1 and less than or equal to n-1, and i is an integer); when n is an even number, at ODnTaking n-1 points H on the line segment1、H2、……Hn-1To make
Figure BDA0001563295610000023
In Hi(i is more than or equal to 1 and less than or equal to n-1, and i is an integer).
Preferably, under the second constraint condition that the arm lengths of the mechanical arms are unequal and no obvious rule exists, the polygon OD1D2…DnMiddle connection DnThe polygon can be divided into n-1 triangles which are not overlapped with other non-adjacent vertexes to form a line segment DnD1，DnD2，…，DnDn-2And the length of each segment of line segment is as follows: dnDi＝ki×DnDi-1Wherein
Figure BDA0001563295610000024
I is not less than 1 and not more than n-2, i is an integer, and when i is 1, D is0Namely the origin O.
The steering engine in the technical scheme adopts PWM pulse width to adjust the rotation angle of the steering engine, the period is 20ms, the pulse width high level of 0.5 ms-2.5 ms corresponds to the angle range of 0-180 degrees of the steering engine, and the linear relation is formed. The steering engine drive control board adopts 500 ~ 2500 numerical value to correspond the high level pulse of 0.5ms ~ 2.5ms of steering engine control output angle, and steering engine drive board numerical value has following relation with the rotatory angle of steering engine: theta is the rotation angle of the steering engine, PWM is the numerical value of a driving plate of the steering engine, and theta is 0.09 multiplied by PW M-45, so that the control precision of the steering engine is 3 mu s, and the minimum control precision can reach 0.3 degree in the range of 2000 pulse widths.
The invention has the beneficial effects that:
1. the posture angle of each limb joint of the mechanical arm is calculated by adding constraint conditions, and the steering engines can work in cooperation by sending numerical signals of PWM values to the steering engines at the same time, so that the center of the mechanical claw can move to a target point quickly and smoothly.
2. Through the change to the constraint condition, the different problems under two different situations of equal arm length and unequal arm length of the mechanical arm are solved by using the same technical scheme.
3. And N (N is more than or equal to 6) is used as the number of the mechanical arm steering engines to give constraint conditions and corresponding parameters, so that a uniform solution is provided for various mechanical arm control methods with the number of the steering engines more than 6, and uniform setting and operation of various mechanical arms are facilitated.
Drawings
FIG. 1 is a schematic diagram of a six-degree-of-freedom rotary mechanical arm steering engine and a system building method.
FIG. 2 is a schematic diagram of a seven-degree-of-freedom rotary mechanical arm steering engine and a system building system.
FIG. 3 is a simplified schematic diagram of a model under a first constraint condition of a six-degree-of-freedom rotary mechanical arm.
FIG. 4 is a simplified schematic diagram of a model of a seven-degree-of-freedom rotary mechanical arm under a first constraint condition.
FIG. 5 is a simplified schematic diagram of a model under a second constraint condition of a six-degree-of-freedom rotary mechanical arm.
FIG. 6 is a simplified model diagram of a seven-degree-of-freedom rotary mechanical arm under a second constraint condition.
Detailed Description
The following describes embodiments of the present invention in further detail with reference to the accompanying drawings.
As shown in fig. 1, 2, 3, 4, 5, and 6, the method for controlling a multi-degree-of-freedom rotary robot arm includes the following steps, step S1: adding constraint conditions to calculate the posture angle of each limb joint of the mechanical arm according to the coordinates of the target point in the three-dimensional rectangular coordinate system; step S2: determining the rotation angle of each steering engine on the mechanical arm according to the attitude angle in the step S1; step S3: determining PWM (pulse width modulation) values of all steering engines according to the rotation angle in the step S2; step S4: and (4) simultaneously sending the numerical value signals of the PWM values of the steering engines in the step (S3) to the steering engines, enabling the steering engines to rotate to the attitude angle in the step (S1), and realizing the coincidence of the center of the mechanical claw and the target point, namely realizing the operation of the mechanical claw to a given three-dimensional space coordinate point.
As shown in fig. 3, 4, 5, and 6, there are two types of constraint conditions added in step S1, which are a first constraint condition with equal arm length of the mechanical arm and a second constraint condition with unequal arm length of the mechanical arm and no obvious regularity.
As shown in attached figures 1 and 2, the multi-degree-of-freedom rotary mechanical arm is provided with N steering engines (N is more than or equal to 6), wherein the steering engine N-1 controls opening and closing of a mechanical claw, the steering engine N-2 controls rotation of the mechanical claw, the steering engine N-N controls rotation of a chassis, and the rest steering engines control movement of limbs of the mechanical arm in a vertical plane; the coordinate of the three-dimensional rectangular coordinate system takes the center of a steering engine N- (N-1) rotating shaft as a coordinate origin O, and an XOY plane of the coordinate system is always parallel to the plane of the mechanical arm base; steering engine N- (N-2) and steering engine N- (N-1) rotating shaft distance OD1Is L1Steering engine N- (N-3) and steering engine N- (N-2) rotating shaft distance D1D2Is L2By parity of reasoning, the distance D from the center of the mechanical claw to the N-3 rotating shaft of the steering enginenDn-1Is Ln. When the target point is known as (x)0,y0,z0) When is at DnThe point coordinate is (x)0,y0,z0) Converting the coordinates into polar coordinates (r) in three-dimensional space0,θ0,φ0)(r0≤L1+L2+L3). At this time, the polygon OD1D2…DnThe shapes are not fixed, and each shape corresponds to the posture of the mechanical claw of the mechanical arm when the mechanical claw reaches the target point. There are countless polygonal ODs1D2…DnThe shape of the robot also has countless grabbing postures, and corresponding posture calculation and posture control cannot be carried out. Therefore, we need to add constraints to make the polygon OD1D2…DnThe full constraint, i.e. the corresponding pose of each target point, is uniquely determined so that it can be computationally controlled.
As shown in fig. 3 and 4, the arm length of the mechanical arm is equal under the first constraint condition, i.e. Li＝Ln+1-i(i is not less than 1 and not more than n-1, i is an integer), and when n is an odd number, at ODnTaking n-1 points H on the line segment1、H2、……Hn-1To make
Figure BDA0001563295610000051
Figure BDA0001563295610000052
In Hi(i is more than or equal to 1 and less than or equal to n-1, and i is an integer); when n is an even number, at ODnTaking n-1 points H on the line segment1、H2、……Hn-1To make
Figure BDA0001563295610000053
In Hi(i is more than or equal to 1 and less than or equal to n-1, and i is an integer). At this time, the polygon OD1D2…DnFully constrained, and line segment DnThe axis of symmetry O is a line segment
Figure BDA0001563295610000054
On a straight line. When the polygon OD1D2…DnAfter the full constraint, the rotation angle of the steering engine for controlling the movement of the limbs of the mechanical arm in the vertical plane, namely the polygonal OD can be obtained1D2…DnEach internal angle of (a). The N-N rotation angle of the steering engine is phi0. The PWM values of the N-2 steering engines can be determined according to the rotation angles from the steering engine N-3 to the steering engine N-N, and signals are simultaneously sent to the steering enginesAnd the N-2 steering engines are respectively rotated to corresponding theoretical calculation angles, and the center of the mechanical claw coincides with the target point, so that the operation that the multi-degree-of-freedom rotary mechanical arm automatically reaches a given three-dimensional space coordinate point is realized.
As shown in the attached figures 5 and 6, under the second constraint condition that the arm lengths of the mechanical arms are unequal and no obvious rule exists, the polygon OD1D2…DnMiddle connection DnThe non-adjacent vertices can divide the polygon into n-1 non-overlapping triangles and constrain the length of the connected line segments so that each triangle is fully constrained, and the polygon OD1D2…DnIs also fully constrained. The scheme of dividing the triangle into a plurality of kinds, because of the angle Dn-1DnO does not relate to the rotation angle of the steering engine, does not need to calculate the size of the steering engine, and is connected with D for simplifying later-period calculationnVertex not adjacent to other vertex, there is a line segment DnD1，DnD2，…，DnDn-2And the length of each segment of line segment is as follows: dnDi＝ki×DnDi-1Wherein
Figure BDA0001563295610000055
I is not less than 1 and not more than n-2, i is an integer, and when i is 1, D is0Namely the origin O. Where k isiIs due to the fact that when r is0＝L1+L2+…+LnWhen the mechanical arm mechanical claw reaches the maximum working radius, only k is needediThe value can be taken to fully extend each limb segment of the mechanical arm. This solution also has the constraint of invisibility, namely DnDi-1+DnDi≥Di-1Di(1. ltoreq. i.ltoreq.n-2, i is an integer, and when i is 1, D is0Namely the origin O), calculating to obtain that the coordinates from the target point to the origin are larger than a certain value and k is related toiThe selection and stealth limitations of (a) will be exemplified later. After adding line segments and constraining the lengths of the line segments, each triangle is fully constrained, the side length of each triangle is known, each internal angle of each triangle can be solved according to the side length, and then the polygon OD is solved1D2…DnThe internal angle of the vertical plane is the rotation angle of the steering engine for controlling the movement of the limbs of the mechanical arm in the vertical plane.
As shown in fig. 1 and 3, a control method under a first constraint condition of a six-degree-of-freedom rotary robot arm is taken as an example. The six-degree-of-freedom rotary mechanical arm comprises 6 paths of steering engines, a steering engine 6-1 controls a mechanical claw to open and close, a steering engine 6-2 controls a mechanical claw to rotate, a steering engine 6-6 controls a chassis to rotate, and a steering engine 6-3, a steering engine 6-4 and a steering engine 6-5 control mechanical arm limbs in a vertical plane to move. And establishing a three-dimensional rectangular coordinate system as shown in the figure by taking the center of the rotating shaft of the steering engine 6-5 as an origin coordinate O, wherein an XOY plane is always parallel to the plane of the base of the mechanical arm. The lengths of all limbs of the mechanical arm are known, and the distances OD of the rotating shafts of the steering gears 6-4 and 6-51Is L1Steering engine 6-3 and steering engine 6-4 rotating shaft distance D1D2Is L2Distance D from mechanical claw center to steering engine 6-3 rotating shaft2D3Is L3From the scheme precondition, L1＝L3。
When the target point is known as (x)0,y0,z0) When is at D3The point coordinate is (x)0,y0,z0) Converting the coordinates into polar coordinates (r) in three-dimensional space0,θ0,φ0)(r0≤L1+L2+L3). At OD3Two points H are taken on the line segment1And H2To make
Figure BDA0001563295610000061
Adding constraint D2H2⊥D3O in H2，D1H1⊥D3O in H1At this time, the quadrangle OD1D2D3Is fully constrained, and H1H2Parallel and equal to D1D2As shown in fig. 2, the rotation angles of the steering engine 6-3, the steering engine 6-4 and the steering engine 6-5 are determined, i.e. the angle D is determined3D2D1、∠D2D1O and < D1OD3(θ-∠D1OD3I.e., the angle of rotation of the steering engine 6-5, and theta is known),the results are as follows:
Figure BDA0001563295610000062
Figure BDA0001563295610000063
Figure BDA0001563295610000064
the steering engine has a rotation angle of 6-6 phi0. PWM values of the four steering engines can be determined according to the rotation angles from the steering engine 6-3 to the steering engine 6-6, signals are sent to the four steering engines at the same time, the four steering engines are enabled to rotate to corresponding theoretical calculation angles respectively, the center of the mechanical claw coincides with a target point at the moment, and operation of the six-degree-of-freedom rotary mechanical arm automatically reaching a given three-dimensional space coordinate point is achieved.
As shown in fig. 2 and 4, a control method under a first constraint condition of a seven-degree-of-freedom rotary mechanical arm is taken as an example. The seven-degree-of-freedom rotary mechanical arm comprises 7 paths of steering engines, wherein the steering engine 7-1 controls mechanical claws to open and close, the steering engine 7-2 controls the mechanical claws to rotate, the steering engine 7-7 controls a chassis to rotate, and the steering engine 7-3, the steering engine 7-4, the steering engine 7-5 and the steering engine 7-6 control the mechanical arm limbs in a vertical plane to move. And establishing a three-dimensional rectangular coordinate system as shown in the figure by taking the center of the rotating shaft of the steering engine 7-6 as an origin coordinate O, wherein an XOY plane is always parallel to the plane of the base of the mechanical arm. The lengths of all limbs of the mechanical arm are known, and the distances OD of the rotating shafts of the steering gears 7-5 and 7-61Is L1Steering engine 7-4 and steering engine 7-5 rotating shaft distance D1D2Is L2Steering engine 7-3 and steering engine 7-4 rotating shaft distance D2D3Is L3Distance D from mechanical claw center to steering engine 7-3 rotating shaft3D4Is L4From the scheme precondition, L1＝L4，L2＝L3。
When the target point is known as (x)0,y0,z0) When is at D4Point coordinates of(x0,y0,z0) Converting the coordinates into polar coordinates (r) in three-dimensional space0,θ0,φ0)(r0≤L1+L2+L3). At OD4Three points H are taken on the line segment3、H2And H1To make
Figure BDA0001563295610000071
Adding constraint D3H3⊥D4O in H3，D2H2⊥D4O in H2，D1H1⊥D4O in H1At this time, the pentagonal OD1D2D3D4Fully constrained, and line segment D4The symmetry axis O is a line segment D2H2In a straight line as shown in fig. 4. Solving the rotation angles of the steering engine 7-3, the steering engine 7-4, the steering engine 7-5 and the steering engine 7-6, namely solving the angle D4D3D2、∠D3D2D1、∠D2D1O and < D1OD3(θ-∠D1OD3I.e., the rotation angle of steering engine 7-6, and θ is known), the results are as follows:
Figure BDA0001563295610000072
Figure BDA0001563295610000073
Figure BDA0001563295610000074
Figure BDA0001563295610000081
the rotation angle of the steering engine is 7-7 phi0. According to the rotation from the steering gear 7-3 to the steering gear 7-7The angles can be determined, the PWM values of the five steering engines can be determined, signals are simultaneously sent to the five steering engines, the five steering engines are enabled to respectively rotate to corresponding theoretical calculation angles, the center of the mechanical claw coincides with the target point, and the operation that the seven-degree-of-freedom rotary mechanical arm automatically reaches a given three-dimensional space coordinate point is achieved.
The first constraint condition is added, the control method of the multi-degree-of-freedom rotary mechanical arm is simple to calculate, the error is small, the working space is the same as the maximum working interval controlled by a single steering engine, namely, no loss in the working interval exists, and experiments prove that the method can be realized, but the lengths of the limbs of the symmetrical mechanical arm need to be considered to be equal in the design and manufacture process of the mechanical arm, and the equal degree of the lengths will influence the error from the mechanical claw to a target point.
As shown in fig. 1 and 5, a control method under the second constraint condition of the six-degree-of-freedom rotary robot arm is taken as an example. Connection D3D1Adding constraint D3D1＝k×r0Wherein
Figure BDA0001563295610000082
At this time, the quadrangle OD1D2D3Fully constrained, as shown in fig. 5. Solving the rotation angles of the steering engines 6-3, 6-4 and 6-5, namely solving the angle D3D2D1、∠D2D1O and < D1OD3(θ-∠D1OD3I.e., the rotation angle of steering engine 6-5, and θ is known), the results are as follows:
Figure BDA0001563295610000083
Figure BDA0001563295610000084
Figure BDA0001563295610000085
the steering engine has a rotation angle of 6-6 phi0. According toThe rotation angles from the steering engine 6-3 to the steering engine 6-6 can determine PWM values of the four steering engines, signals are sent to the four steering engines at the same time, the four steering engines are enabled to rotate to corresponding theoretical calculation angles respectively, the centers of the mechanical claws coincide with the target points, and the operation that the six-degree-of-freedom rotary mechanical arm automatically reaches a given three-dimensional space coordinate point is achieved. When r is0＝L1+L2+L3When D is3D1＝k×r0＝L2+L3＝D1D2+D2D3＝D3O-D1O, i.e. D3、D2、D1And O, the robot arm is fully extended. If k takes other values, the mechanical claw of the mechanical arm cannot be fully extended, namely, the mechanical claw cannot work under the maximum working radius, and the working space is reduced. It should be noted that D1D3+D3O≥D1O, i.e.
Figure BDA0001563295610000091
That is, the distance of the target point to the origin cannot be less than
Figure BDA0001563295610000092
As shown in fig. 2 and 6, a control method under the second constraint condition of the seven-degree-of-freedom rotary mechanical arm is taken as an example. Connection D4D1And D4D2Adding constraint D4D1＝k1×r0，D4D2＝k2×D4D1Wherein
Figure BDA0001563295610000093
At this time, the pentagonal OD1D2D3D4Fully constrained, as shown in fig. 6. Solving the rotation angles of the steering engine 7-3, the steering engine 7-4, the steering engine 7-5 and the steering engine 7-6, namely solving the angle D4D3D2、∠D3D2D1、∠D2D1O and < D1OD3(θ-∠D1OD3I.e., the angle of rotation of the steering engine 6-5, and theta is known), the results are as followsThe following:
Figure BDA0001563295610000094
Figure BDA0001563295610000095
Figure BDA0001563295610000096
Figure BDA0001563295610000097
the rotation angle of the steering engine is 7-7 phi0. The PWM values of the five steering engines can be determined according to the rotation angles from the steering engine 7-3 to the steering engine 7-7, signals are sent to the five steering engines at the same time, the five steering engines are enabled to rotate to corresponding theoretical calculation angles respectively, the center of the mechanical claw coincides with a target point at the moment, and the operation that the seven-degree-of-freedom rotary mechanical arm automatically reaches a given three-dimensional space coordinate point is achieved. It should be noted that D1D4+D4O≥D1O,D2D4+D4D1≥D2D1I.e. by
Figure BDA0001563295610000098
And is
Figure BDA0001563295610000099
The second constraint condition is added, the control method of the multi-degree-of-freedom rotary mechanical arm is good in applicability, simple in calculation and almost 0 in error, and is particularly suitable for controlling mechanical arms with irregular lengths of limbs and joints. The working space is smaller than the working space under the control of a single steering engine, and the distance from a target point to an original point cannot be smaller than a fixed value related to the length of the limb joint of the mechanical arm because of the existence of a recessive constraint condition. However, since the certain value can be adjusted by manually changing the length of a certain limb of the mechanical arm, and the area of the target point which cannot be reached by the mechanical gripper is usually small, and the main working area of the mechanical arm is not affected, the second constraint condition is not lost as a good choice for adding the constraint condition.
When the center coordinate (x) of the mechanical claw of the current multi-degree-of-freedom rotary mechanical arm is known0,y0,z0) While, reading the input state, e.g. receiving z0Reduced instruction, the target point coordinate is (x)0,y0,z0-P) (P is the step length, the size of the step length can be programmed and adjusted to realize the adjustment of the movement speed of the mechanical claw), and then the mechanical claw of the mechanical arm reaches a target point through the multi-degree-of-freedom rotary mechanical arm fixed point movement algorithm. At this time the target point will be taken as z0Reducing the center coordinate (x) of the mechanical claw of the rear multi-freedom-degree rotary mechanical arm0,y0,z0) The coordinates are refreshed, and the steps are repeated at each moment, and the same principle is added and subtracted along the other coordinate axes, so that the center of the mechanical claw of the multi-freedom-degree rotary mechanical arm moves along a straight line.
When the mechanical claw grabs an object in a direction, the steering engine N-3 used for the movement of the limbs of the mechanical arm in the vertical plane is idle at the moment and used for adjusting the posture of the mechanical claw, so that the mechanical claw has two spatial degrees of freedom and can complete the grabbing in any direction in a working plane. The steering engine N-1 controls the mechanical claw to open and close, the steering engine N-2 and the steering engine N-3 control the mechanical claw to rotate, the steering engine N-N controls the chassis to rotate, and the other steering engines control the mechanical arm limbs in the vertical plane to move. The steering engine N-1, the steering engine N-2 and the steering engine N-3 are used for mechanical claw grabbing control, and the motion of the mechanical claws is not influenced, so that the steering engine N-1, the steering engine N-2 and the steering engine N-3 are considered independently and are out of the research range of the invention. When the grabbing direction of the mechanical claw is considered, the aim of controlling the movement of the mechanical arm is to enable the coordinate of a target point to coincide with the center of a rotating shaft N-3 of the steering engine, and at the moment, the number of the steering engines moving on the limbs of the mechanical arm in a vertical plane is changed from N to N-1, namely, the steering engines move from a polygon OD1D2…DnBecomes a polygonal OD1D2…Dn-1The control method can still refer to the two methods, so that the motion control of the multi-degree-of-freedom rotary mechanical arm considering the direction of the mechanical claw for grabbing the object is completed.
The above description is only a preferred embodiment of the present invention, but the scope of the present invention is not limited thereto, and any design idea of calculating the posture angle of each limb joint of the mechanical arm by adding the constraint condition of the present invention according to the coordinate of the target point in the three-dimensional rectangular coordinate system, so as to enable the gripper to directly reach the target point, thereby achieving the fixed-point automatic object grabbing by the multi-degree-of-freedom rotary gripper belongs to the scope of the present invention.

Claims (1)

1. The control method of the multi-degree-of-freedom rotary mechanical arm is characterized by comprising the following steps of:
step S1: adding constraint conditions to calculate the posture angle of each limb joint of the mechanical arm according to the coordinates of the target point in the three-dimensional rectangular coordinate system;
step S2: determining the rotation angle of each steering engine on the mechanical arm according to the attitude angle in the step S1;
step S3: determining PWM (pulse width modulation) values of all steering engines according to the rotation angle in the step S2;
step S4: simultaneously sending the numerical value signals of the PWM values of the steering engines in the step S3 to the steering engines, enabling the steering engines to rotate to the attitude angle in the step S1, realizing the coincidence of the center of the mechanical claw and the target point, namely realizing the operation of the mechanical claw to a given three-dimensional space coordinate point;
the multi-degree-of-freedom rotary mechanical arm is provided with n paths of steering engines, wherein n is more than or equal to 6, the steering engine n __1 controls opening and closing of a mechanical claw, the steering engine n __2 controls rotation of the mechanical claw, the steering engine n __ n controls rotation of a chassis, and the other steering engines control movement of limbs of the mechanical arm in a vertical plane; the coordinates of the three-dimensional rectangular coordinate system take the center of a rotating shaft of a steering engine n __ (n-1) as a coordinate origin O, and an XOY plane of the coordinate system is always parallel to the plane of the mechanical arm base; steering engine n __ (n-2) and steering engine n __ (n-1) rotating shaft distance OD1Is L1Steering engine n __ (n-3) and steering engine n __ (n-2) rotating shaft distance D1D2Is L2By analogy, mechanical clawDistance D from center to rotating shaft of steering engine n __3nDn-1Is Ln；
The constraint conditions added in the step S1 are two, namely a first constraint condition with equal arm length of the mechanical arm and a second constraint condition with unequal arm length of the mechanical arm and no obvious rule;
under the first constraint of equal arm length of the mechanical arm, namely Li＝Ln+1-iWhere 1. ltoreq. i.ltoreq.n-1, i is an integer, when the target point is known as (x)0,y0,z0) When is at DnThe point coordinate is (x)0,y0,z0) Converting the coordinates into polar coordinates (r) in three-dimensional space0,θ0,φ0) Wherein r is0≤L1+L2+L3(ii) a When n is an odd number, at ODnTaking n-1 points H on the line segment1、H2、……Hn-1To make
Figure FDA0002905757880000021
Wherein
Figure FDA0002905757880000022
DiHi⊥DnO in HiWherein i is more than or equal to 1 and less than or equal to n-1, and i is an integer; when n is an even number, at ODnTaking n-1 points H on the line segment1、H2、……Hn-1To make
Figure FDA0002905757880000023
DiHi⊥DnO in HiWherein i is more than or equal to 1 and less than or equal to n-1, and i is an integer;
under the second constraint condition that the arm lengths of the mechanical arms are unequal and no obvious rule exists, the polygon OD1D2…DnMiddle connection DnThe polygon can be divided into n-1 triangles which are not overlapped with other non-adjacent vertexes to form a line segment DnD1，DnD2，…，DnDn-2And the length of each segment of line segment is as follows: dnDi＝ki×DnDi-1Wherein
Figure FDA0002905757880000024
i is an integer, and when i is 1, D is0Namely the origin O.
CN201810089456.8A 2018-01-30 2018-01-30 Control method of multi-degree-of-freedom rotary mechanical arm Expired - Fee Related CN108247638B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810089456.8A CN108247638B (en) 2018-01-30 2018-01-30 Control method of multi-degree-of-freedom rotary mechanical arm

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810089456.8A CN108247638B (en) 2018-01-30 2018-01-30 Control method of multi-degree-of-freedom rotary mechanical arm

Publications (2)

Publication Number Publication Date
CN108247638A CN108247638A (en) 2018-07-06
CN108247638B true CN108247638B (en) 2021-03-26

Family

ID=62743084

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810089456.8A Expired - Fee Related CN108247638B (en) 2018-01-30 2018-01-30 Control method of multi-degree-of-freedom rotary mechanical arm

Country Status (1)

Country Link
CN (1) CN108247638B (en)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109287578B (en) * 2018-11-08 2023-07-04 中国农业大学 Sea cucumber catching system
CN110142756A (en) * 2019-03-29 2019-08-20 盐城工学院 A kind of six-joint robot that can be achieved to be precisely controlled
CN111498029B (en) * 2020-04-23 2022-01-14 山东大学 Auxiliary device is catched to seabed
CN112476437B (en) * 2020-12-08 2021-11-30 北京航天新立科技有限公司 Flexible automatic grabbing equipment control method
CN113580140A (en) * 2021-08-18 2021-11-02 连江福桔电子有限公司 Control method and terminal of seven-axis mechanical arm
CN114700956A (en) * 2022-05-20 2022-07-05 江苏金和美机器人科技有限公司 Identification, positioning and gripping device and method for robot-oriented article gripping operation

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
FR2434685A1 (en) * 1978-09-04 1980-03-28 Commissariat Energie Atomique MOTORIZED MANIPULATOR
KR100924505B1 (en) * 2002-01-18 2009-11-02 혼다 기켄 고교 가부시키가이샤 Controller of legged mobile robot
CN102208150B (en) * 2011-05-13 2013-03-20 武汉理工大学 Six-degree-of-freedom force-feedback virtual surgical instrument
CN203452682U (en) * 2013-08-23 2014-02-26 四川宏华石油设备有限公司 Drilling tool carrying manipulator
CN105123637B (en) * 2015-06-23 2017-08-25 山东科技大学 A kind of assimilating type finishing device and its method of work under water
CN105014677B (en) * 2015-07-07 2016-07-20 西安交通大学 Vision Mechanical arm control method based on Camshift visual tracking and D-H modeling algorithm

Also Published As

Publication number Publication date
CN108247638A (en) 2018-07-06

Similar Documents

Publication Publication Date Title
CN108247638B (en) Control method of multi-degree-of-freedom rotary mechanical arm
JP6751399B2 (en) Underwater manipulator arm robot
CN108196445B (en) Underwater robot posture and position control method based on double closed-loop active disturbance rejection
CN108490961B (en) Multi-AUV dynamic arc formation control method
JP2014193520A (en) Multi-axis robot trajectory forming method and multi-axis robot control device
CN111390872A (en) Double-arm cooperative flexible dragging and butt joint inverse operation method for extravehicular robot
JP2014193519A (en) Multi-axis robot trajectory forming method and multi-axis robot control device
CN110937092A (en) CPG control method of multi-mode bionic stay wire robot fish and robot fish
CN112947462A (en) Unmanned ship group formation cooperative control method considering time-varying drift angle and attitude adjustment
WO2023173764A1 (en) Fusion system of mechanical arm and dexterous hand, and movement control method therefor
Sverdrup-Thygeson et al. A control framework for biologically inspired underwater swimming manipulators equipped with thrusters
CN102411307A (en) Fast and precise motion control method of C-type starting of bio-mimetic robot fish
Wang et al. Path following for a biomimetic underwater vehicle based on ADRC
CN107253191A (en) A kind of double mechanical arms system and its control method for coordinating
CN207752374U (en) AUV Control device
Manerikar et al. On autonomous cooperative underwater floating manipulation systems
CN113296524B (en) Thrust vector distribution optimization method for underwater bionic spherical/hemispherical robot
CN116330267A (en) Control method based on industrial robot wrist singular point calculation
Liljebäck et al. Waypoint guidance control of snake robots
JP6123595B2 (en) Speed control method for 2-axis robot
CN116652958A (en) Master-slave isomorphic robot teleoperation safety control method and system
Zhang et al. 2-DOF camera stabilization platform for robotic fish based on active disturbance rejection control
CN109533233B (en) Self-adaptive control method of underwater robot propeller
Tang et al. RobCutt: a framework of underwater biomimetic vehicle-manipulator system for autonomous interventions
Liu et al. Flippers-based turning analysis and implementation of a dolphin robot

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
GR01 Patent grant
GR01 Patent grant
CF01 Termination of patent right due to non-payment of annual fee
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20210326

Termination date: 20220130