CN111993414B - Mechanical arm multi-joint linkage control method - Google Patents

Mechanical arm multi-joint linkage control method Download PDF

Info

Publication number
CN111993414B
CN111993414B CN202010737493.2A CN202010737493A CN111993414B CN 111993414 B CN111993414 B CN 111993414B CN 202010737493 A CN202010737493 A CN 202010737493A CN 111993414 B CN111993414 B CN 111993414B
Authority
CN
China
Prior art keywords
arm
joint
arm support
mechanical arm
coordinate system
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
Application number
CN202010737493.2A
Other languages
Chinese (zh)
Other versions
CN111993414A (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.)
Beijing Xuanyu Intelligent Technology Co ltd
Original Assignee
Beijing Xuanyu Intelligent Technology Co ltd
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 Beijing Xuanyu Intelligent Technology Co ltd filed Critical Beijing Xuanyu Intelligent Technology Co ltd
Priority to CN202010737493.2A priority Critical patent/CN111993414B/en
Publication of CN111993414A publication Critical patent/CN111993414A/en
Application granted granted Critical
Publication of CN111993414B publication Critical patent/CN111993414B/en
Active 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/1628—Programme controls characterised by the control loop
    • B25J9/163—Programme controls characterised by the control loop learning, adaptive, model based, rule based expert control
    • 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/1661—Programme controls characterised by programming, planning systems for manipulators characterised by task planning, object-oriented languages

Landscapes

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

Abstract

The invention discloses a multi-joint linkage control method for a mechanical arm, which comprises the following steps of firstly, constructing a mechanical arm control system frame, wherein the control system frame comprises a control layer, a model layer and a planning layer; secondly, constructing a mechanical arm fixing base coordinate system, a coordinate system of a plurality of arm support joints and mathematical models of the plurality of arm support joints under different configurations on the model layer; then, constructing a kinematic inverse solution algorithm of the mechanical arm under different configurations at the control layer based on the mathematical models of the joints of the plurality of arm supports and the use working conditions of the mechanical arm; and finally, planning a track function of each arm support joint of the mechanical arm on a planning layer based on a kinematics inverse solution algorithm. The control method and the control system framework can be popularized to general engineering machinery, and high-precision and intelligent control of the engineering machinery is realized.

Description

Mechanical arm multi-joint linkage control method
Technical Field
The invention belongs to the field of engineering machinery, and particularly relates to a mechanical arm multi-joint linkage control method.
Background
The control of the existing vehicle-mounted engineering mechanical arm can be divided into single-joint control and multi-joint linkage control based on terminal motion, wherein the first mode is widely applied to various engineering machines such as excavators and concrete pump trucks, and the second mode is mainly used for automatically planning the posture and the speed of each mechanical arm support by giving the moving speed and the moving direction of a hose at the tail end of the mechanical arm of the concrete pump truck with an intelligent arm support technology.
At present, a pump truck with an intelligent boom technology is generally controlled by a remote controller, but at present, the control method is only applied to a planar engineering machine with few degrees of freedom or redundant degrees of freedom, such as a concrete pump truck, of which all boom are rotating joints, and can only realize open-loop position and attitude control under manual assistance intervention, so that the control precision is low, the operation is complicated, and the intuition is poor.
In addition, the control mode adopted by some engineering mechanical arms is open-loop control, only aims at the condition that the target position is known, and cannot be suitable for high-precision intelligent control of the engineering mechanical arms under the condition of teleoperation.
Therefore, how to provide a high-precision intelligent control system and a control method suitable for the boom-type spatial redundancy freedom engineering machinery becomes a technical problem to be solved urgently.
Disclosure of Invention
Aiming at the problems, the invention provides a mechanical arm multi-joint linkage control method which is strong in universality and higher in control precision.
The invention aims to provide a multi-joint linkage control method for a mechanical arm, which comprises the following steps,
constructing a mechanical arm control system frame, wherein the control system frame comprises a control layer, a model layer and a planning layer;
constructing a mechanical arm fixing base coordinate system, a coordinate system of a plurality of arm support joints and a plurality of arm support joint mathematical models on the model layer;
constructing a kinematics inverse solution algorithm of the mechanical arm at the control layer based on the mathematical models of the joints of the plurality of arm supports and the use condition of the mechanical arm;
and planning a track function of each arm support joint of the mechanical arm on a planning layer based on a kinematics inverse solution algorithm.
Further, constructing a mechanical arm fixing base coordinate system, a coordinate system of a plurality of arm support joints and a plurality of arm support joint mathematical models on the model layer comprises,
establishing a fixed base coordinate system at a rotary base of the mechanical arm;
determining the mechanical arm configuration of the mechanical arm, and acquiring the elongation and joint angle of each arm support joint in a plurality of arm support joints in the mechanical arm;
and establishing a coordinate system of the plurality of arm support joints based on the base coordinate system, the configuration of the mechanical arm, the elongation and the joint corner of each arm support joint, and establishing a mathematical model of the plurality of arm support joints by using a DH method.
Further, the mechanical arm configuration comprises a redundant mechanical arm and a fixed-rod-length constraint mechanical arm, wherein,
when the mechanical arm is a redundant mechanical arm, the redundant mechanical arm comprises six revolute pairs and one moving pair, six degrees of freedom in space are provided, and the coordinate systems of the arm frames meet the following requirements:
the coordinate system of the first arm support joint to the seventh arm support joint comprises an X axis and a Z axis, and the coordinate system of the eighth arm support joint comprises an X axis, a Y axis and a Z axis;
the mathematical models of the arm support joints satisfy that:
in an initial state, the included angle of the coordinate systems of any two adjacent arm support joints around the Z axis is 0;
d is the deviation of the X axis of the coordinate system of the second arm support and the third arm support joint, the coordinate system of the fourth arm support and the fifth arm support joint, the coordinate system of the sixth arm support and the seventh arm support joint and the coordinate system of the seventh arm support and the eighth arm support joint in the Z axis direction respectively3、d6、d7、d8；
The offset of the Z axis of the coordinate system of the first arm support and the second arm support joint, the coordinate system of the second arm support joint and the third arm support joint, the coordinate system of the fourth arm support and the fifth arm support joint and the offset of the Z axis of the coordinate system of the fifth arm support and the sixth arm support joint in the X axis direction are respectively-a1、a2、-a4、-a5；
The telescopic distance of the sliding pair is L4;
when the mechanical arm configuration is a fixed rod length constraint mechanical arm, the fixed rod length constraint mechanical arm comprises six revolute pairs and has six spatial degrees of freedom, and the coordinate systems of the joints of the arm frames meet the following requirements:
the coordinate system of the first arm support joint to the sixth arm support joint comprises an X axis and a Z axis, and the coordinate system of the seventh arm support joint comprises an X axis, a Y axis and a Z axis;
the mathematical models of the arm support joints satisfy that:
in an initial state, the included angle of the coordinate systems of any two adjacent arm frame joints around the Z axis is 0;
d is the deviation of the X axis of the coordinate system of the second arm support and the third arm support joint, the coordinate system of the fourth arm support and the fifth arm support joint, the coordinate system of the fifth arm support and the sixth arm support joint and the coordinate system of the sixth arm support and the seventh arm support joint in the Z axis direction respectively3、d5、d6、d7；
The offset of the Z axis of the coordinate system of the first arm support and the second arm support joint, the coordinate system of the second arm support and the third arm support joint, the coordinate system of the third arm support and the fourth arm support joint and the coordinate system of the fourth arm support and the fifth arm support joint in the X axis direction is respectively-a1、a2、a3、a4；
In the initial state, the telescopic distance of the moving pair is 0.
Further, the inverse kinematics solution algorithm comprises a redundant degree of freedom numerical solution and a spatial six-degree-of-freedom analytic solution.
Further, the building of the inverse kinematics solution algorithm of the mechanical arm at the control layer based on the mathematical models of the plurality of arm support joints and the use conditions of the mechanical arm includes building a redundant degree of freedom numerical solution, wherein the building includes,
setting the pose of the tail end of the redundant mechanical arm as (x, y, Z, alpha, beta, gamma) based on a mathematical model of a plurality of arm frame joints, wherein (x, y, Z) represents the position of the tail end of the mechanical arm in space, and (alpha, beta, gamma) represents the posture of the tail end of the mechanical arm in space, and the included angles of joint coordinate systems of two adjacent arm frames from a rotary base to a third arm frame and from a fourth arm frame to a seventh arm frame in the redundant mechanical arm around a Z axis are respectively theta1,θ2,θ3,θ5,θ6,θ7And the cylinder body elongation of each driving cylinder in the redundant mechanical arm is l2,l3,l4,l5,l6；
The following functional relationships are respectively determined:
pose and theta of the end of the redundant manipulator1,θ2,θ3,θ5,θ6,θ7And l4Functional relationship of (1), theta in redundant robot arm2,θ3,θ5,θ6Are respectively corresponding to2,l3,l5,l6Functional relation between the end speed of the redundant mechanical arm and the angular speed of the joint from the first arm frame joint to the seventh arm frame joint in the redundant mechanical arm, and the redundant mechanical arm
Figure GDA0003299672220000033
And
Figure GDA0003299672220000031
in a functional relationship therebetween, wherein,
Figure GDA0003299672220000034
the telescopic speed of a driving cylinder between a first arm support and a second arm support, the telescopic speed of a driving cylinder between the second arm support and a third arm support, the telescopic speed of a driving cylinder between a fourth arm support and a fifth arm support, and the telescopic speed of a driving cylinder between the fifth arm support and a sixth arm support;
Figure GDA0003299672220000032
joint angular velocities of the second boom joint, the third boom joint, the fifth boom joint, and the sixth boom joint.
Further, the pose and the theta of the end of the redundant mechanical arm1,θ2,θ3,θ5,θ6,θ7And l4Functional relationship of (a):
x＝f(θ1,θ2,θ3,l4,θ5,θ6,θ7)
y＝f(θ1,θ2,θ3,l4,θ5,θ6,θ7)
z＝f(θ1,θ2,θ3,l4,θ5,θ6,θ7)
α＝f(θ1,θ2,θ3,l4,θ5,θ6,θ7)
β＝f(θ1,θ2,θ3,l4,θ5,θ6,θ7)
γ＝f(θ1,θ2,θ3,l4,θ5,θ6,θ7)；
theta in the redundant robot arm2,θ3,θ5,θ6Are respectively corresponding to2,l3,l5,l6Functional relationship between:
θ2＝g(l2)
θ3＝g(l3)
θ5＝g(l5)
θ6＝g(l6)；
the functional relation between the speed of the tail end of the redundant mechanical arm and the angular speeds of the joints from the first arm frame joint to the seventh arm frame joint in the redundant mechanical arm is as follows:
Figure GDA0003299672220000041
wherein the content of the first and second substances,
Figure GDA0003299672220000042
representing the velocity component of the redundant manipulator tip velocity in the pose,
Figure GDA0003299672220000043
representing first to seventh jib jointsAngular velocity of joint, fx、fy、fγRepresenting dependent variables of all parameters in the pose;
in the redundant mechanical arm
Figure GDA0003299672220000049
And
Figure GDA0003299672220000044
functional relationship between:
Figure GDA0003299672220000045
Figure GDA0003299672220000046
Figure GDA0003299672220000047
Figure GDA0003299672220000048
further, based on the mathematical models of the joints of the arm support and the use conditions of the mechanical arm, constructing a kinematic inverse solution algorithm of the mechanical arm at the control layer further comprises constructing a spatial six-degree-of-freedom analytical solution, wherein the method comprises the following steps of,
setting the pose of the fixed rod length constraint mechanical arm tail end as (p) based on a mathematical model of a plurality of arm support jointsx,py,pzα, β, γ), wherein (p)x,py,pz) The tail end position of the tail end of the fixed rod length constraint mechanical arm in the space is shown, (alpha, beta, gamma) shows the posture of the tail end of the fixed rod length constraint mechanical arm in the space, and the included angles of joint coordinate systems of two adjacent arm frames from a first arm frame to a fourth arm frame in the fixed rod length constraint mechanical arm around a Z axis are theta2,θ3,θ4And a first arm support and a fourth arm support in the constraint mechanical arm with fixed rod lengthThe cylinder body elongation of the driving cylinder between the arm supports is l2,l3,l4；
Two representation modes are adopted to represent the pose of the tail end of the mechanical arm, wherein,
the first expression is as follows: at any moment, the pose of the tail end of the fixed rod length constraint mechanical arm is expressed as a matrix T under a base coordinate system:
Figure GDA0003299672220000051
wherein n isx、ny、nz、ox、oy、oz、ax、ay、azFor representing attitude parameters, p, of the pose of the end of the fixed-bar length constrained manipulatorx、py、pzThe position parameter is a position parameter representing the position of the tail end of the fixed rod length constraint mechanical arm in space;
the second expression mode is as follows: based on a fixed rod length constraint mechanical arm mathematical model, using theta for each item in the matrix TiRepresenting, obtaining, matrix Tθ：
Figure GDA0003299672220000052
Determining an included angle theta of the coordinate system of two adjacent arm support joints of the fixed rod length constraint mechanical arm from the first arm support joint to the sixth arm support joint around the Z axis based on the first representation mode and the second representation mode1,θ2,θ3,θ4,θ5,θ6The kinematic analysis expression of (a);
the following functional relationships are respectively determined:
theta in fixed rod length constraint mechanical arm2,θ3,θ4Are respectively corresponding to2,l3,l4The functional relationship between the tail end speed of the fixed rod length constraint mechanical arm and the joint angular speed from the first arm frame joint to the seventh arm frame joint in the fixed rod length constraint mechanical arm, and the fixed rod length constraintIn the binding mechanical arm
Figure GDA0003299672220000065
And
Figure GDA0003299672220000061
in a functional relationship therebetween, wherein,
Figure GDA0003299672220000066
the telescopic speed of a driving cylinder between the first arm support and the second arm support, the telescopic speed of a driving cylinder between the second arm support and the third arm support and the telescopic speed of a driving cylinder between the third arm support and the fourth arm support are set;
Figure GDA0003299672220000062
joint angular velocities of the second boom joint, the third boom joint, and the fourth boom joint.
Further, based on the first representation mode and the second representation mode, an included angle theta between coordinate systems of two adjacent arm support joints of the fixed rod length constraint mechanical arm from the first arm support joint to the sixth arm support joint around the Z axis is determined1,θ2,θ3,θ4,θ5,θ6The kinematic analysis expression specifically comprises the following steps:
step 1, based on the deviation d of the X axis of the coordinate system of the first arm support and the second arm support joint and the second arm support and the third arm support joint in the Z axis direction2、d3Making the matrix elements (2,4) correspondingly equal in the two expression modes, and solving the included angle theta between the first arm support joint and the rotary base around the Z axis1The kinematic analytical expression of (a):
Figure GDA0003299672220000063
wherein the matrix elements (2,4) represent the elements of the 2 nd row and the 4 th column in the matrix,
step 2, making the matrix elements (2,3) under the two expression modes correspondingly equal to obtain a fourth arm support andthe included angle theta of the fifth arm support joint coordinate system around the Z axis5The kinematic analytical expression of (a):
θ5＝±arccos(axs1-ayc1)
where the matrix elements (2,3) represent the elements in row 2 and column 3 of the matrix, c1Represents cos (theta)1)，s1Represents sin (theta)1)；
Step 3, correspondingly equalizing the matrix elements (2,2) in the two expression modes, and solving an included angle theta of a fifth arm support and a sixth arm support joint coordinate system around the Z axis6The kinematic analytical expression of (a):
Figure GDA0003299672220000064
where the matrix elements (2,2) represent the elements of row 2 and column 2 in the matrix, c1Represents cos (theta)1)，s1Represents sin (theta)1)；
Step 4, based on the coordinate system of the second arm support and the third arm support joint and the deviation a of the Z axis of the coordinate system of the third arm support and the fourth arm support joint in the X axis direction2、a3Respectively and correspondingly equalizing the matrix elements (1,4) and (3,4) in the two expression modes, unfolding and shifting the intermediate items, and sequentially obtaining the included angle theta of the coordinate systems of the first arm support and the second arm support joint and the second arm support and the third arm support joint around the Z axis2、θ3The kinematic analytical expression of (a):
Figure GDA0003299672220000071
θ2＝Atan2(s2，c2)
wherein the matrix elements (1,4) represent the elements of the 1 st row and 4 th column of the matrix, the matrix elements (3,4) represent the elements of the 3 rd row and 4 th column of the matrix, c2Represents cos (theta)2)，s2Represents sin (theta)2)；
Step 5, based on the included angle theta of the coordinate systems of the first arm support and the second arm support joint and the second arm support and the third arm support joint around the Z axis4Respectively and correspondingly equalizing matrix elements (1,3) and (3,3) in the two expression modes, unfolding and shifting the intermediate items, and solving an included angle theta of a joint coordinate system of the third arm support and the fourth arm support around the Z axis4The kinematic analytical expression of (a):
θ4＝Atan2(-s6(nxc1+nys1)-c6(oxc1+oys1)，ozc6+nzs6)-θ2-θ3
wherein the matrix elements (1,3) represent the elements of the 1 st row and 3 rd column in the matrix, the matrix elements (3,3) represent the elements of the 3 rd row and 3 rd column in the matrix, c1Represents cos (theta)1)，s1Represents sin (theta)1)，c6Represents cos (theta)6)，s6Represents sin (theta)6)。
Further, the fixed rod length restrains theta in the mechanical arm2,θ3,θ4Are respectively corresponding to2,l3,l4Functional relationship between:
θ2＝g(l2)
θ3＝g(l3)
θ4＝g(l4)
the function relation between the speed of the tail end of the fixed rod length constraint mechanical arm and the angular speeds of the joints from the first arm frame joint to the seventh arm frame joint in the fixed rod length constraint mechanical arm is as follows:
Figure GDA0003299672220000081
wherein J is a Jacobian matrix represented by the constraint mechanical arm with fixed rod length under a base coordinate system,
Figure GDA0003299672220000082
representing the velocity component of the redundant manipulator tip velocity in the pose,
Figure GDA0003299672220000083
representing joint angular velocities of the first arm support joint to the seventh arm support joint;
in the fixed rod length constraint mechanical arm
Figure GDA0003299672220000086
And
Figure GDA0003299672220000084
functional relationship between:
Figure GDA0003299672220000085
further, planning the track function of each arm support joint of the mechanical arm in a planning layer comprises,
acquiring a motion direction and a speed signal of the tail end of the mechanical arm from a current point to a target point;
calculating the current position and posture information of the tail end of the mechanical arm based on a mechanical arm fixing base coordinate system, a coordinate system and a mathematical model of a plurality of arm support joints, joint signals and control instructions;
calculating a track function from the current point to the target point of the tail end of the mechanical arm based on the control instruction;
and calculating a track function corresponding to each arm support joint in the motion process of the mechanical arm based on a kinematics inverse solution algorithm.
Further, the control method may further include,
receiving a control instruction sent by a control device;
judging the configuration of the mechanical arm, determining a kinematics inverse solution algorithm at a control layer, wherein,
when the configuration of the mechanical arm is a redundant mechanical arm, selecting a redundant degree of freedom numerical solution at a control layer;
and when the configuration of the mechanical arm is a fixed rod length constraint mechanical arm, selecting a spatial six-degree-of-freedom analytic solution at the control layer.
The mechanical arm multi-joint linkage control method has better universality, is not only suitable for a planar full-rotation joint less-freedom-degree or redundant mechanical arm, but also suitable for a space redundant mechanical arm containing a biased moving auxiliary arm frame, and can simulate the mechanical arm space multi-freedom-degree motion more intuitively. In addition, the constructed control system framework also realizes the high-precision end motion control of the real-time closed loop of the engineering mechanical arm, so that the operation is simpler and more visual, the controllability is better, and the control precision is higher.
Additional features and advantages of the invention will be set forth in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. The objectives and other advantages of the invention will be realized and attained by the structure particularly pointed out in the written description and claims hereof as well as the appended drawings.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below, and it is obvious that the drawings in the following description are some embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to these drawings without creative efforts.
FIG. 1 is a schematic diagram of a robot frame system according to an embodiment of the present invention;
FIG. 2 is a schematic diagram of a redundant robot arm base coordinate system and coordinate systems of a plurality of boom joints according to an embodiment of the present invention;
fig. 3 is a schematic diagram illustrating a base coordinate system of a mechanical arm and coordinate systems of a plurality of arm support joints with a fixed rod length according to an embodiment of the present invention;
FIG. 4 shows two adjacent arms of a constraint mechanical arm from a first arm support to a sixth arm support for determining the length of a fixed rod in the embodiment of the inventionAngle theta around Z axis of frame joint coordinate system1,θ2,θ3,θ4,θ5,θ6The kinematic analysis expression of (a);
FIG. 5 is a schematic flow diagram illustrating a portion of a redundant robotic arm control methodology in accordance with an embodiment of the present invention;
fig. 6 is a schematic diagram illustrating a trajectory function of a first arm joint to a seventh arm joint of a redundant manipulator according to an embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are some, but not all, embodiments of the present invention. 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 invention.
The embodiment of the invention discloses a multi-joint linkage control method for a mechanical arm, which comprises the following steps of firstly, constructing a mechanical arm control system frame, wherein the control system frame comprises a control layer, a model layer and a planning layer; secondly, constructing a mechanical arm fixing base coordinate system, a coordinate system of a plurality of arm support joints and a plurality of arm support joint mathematical models on the model layer; then, constructing a kinematics inverse solution algorithm of the mechanical arm at the control layer based on the mathematical models of the joints of the plurality of arm supports and the use condition of the mechanical arm; and finally, planning a track function of each arm support joint of the mechanical arm on a planning layer based on a kinematics inverse solution algorithm. The control method realizes the high-precision end motion control of the real-time closed loop of the engineering mechanical arm, so that the operation is simpler and more visual, the controllability is better, and the control precision is higher.
Illustratively, as shown in fig. 1, the mechanical arm control system framework includes a control layer, a model layer and a planning layer, wherein the control layer is provided with a redundant degree of freedom numerical solution and a spatial six-degree-of-freedom analytic solution, and the model layer is provided with a redundant mechanical arm mathematical model and a fixed rod length constraint mechanical arm mathematical model. The planning layer includes CP path planning and joint velocity planning. In the embodiment of the invention, the track function of each arm support joint can be obtained based on the CP path planning and the joint speed planning.
In the embodiment, the construction of the mechanical arm fixed base coordinate system, the coordinate systems of the plurality of arm support joints and the plurality of arm support joint mathematical models on the model layer comprises the steps of firstly, establishing a fixed base coordinate system at a rotary base of the mechanical arm; then, determining the mechanical arm configuration of the mechanical arm, and acquiring the elongation and joint angle of each arm support joint in a plurality of arm support joints in the mechanical arm; and finally, establishing a coordinate system of a plurality of arm support joints based on the base coordinate system, the configuration of the mechanical arm, the elongation and the joint corner of each arm support joint, and establishing a mathematical model of the plurality of arm support joints by using a DH method. The mechanical arm configuration refers to an arrangement mode of the mechanical arms, wherein the arrangement mode of the mechanical arms comprises redundant mechanical arms and fixed rod length constraint mechanical arms. Under different configurations, the mathematical models and the inverse kinematics solution algorithms of a plurality of arm support joints are different, and the constructed corresponding coordinate systems are also different.
When the robot arm configuration is a redundant robot arm, the redundant robot arm shown in fig. 2 is exemplarily illustrated, and the redundant robot arm includes six revolute pairs and one revolute pair, and has six degrees of freedom in space, so that X0 and Z0 in the coordinate system of the swing base of the robot arm (i.e., a base coordinate system) shown in fig. 2 are the X axis and the Z axis of the swing base of the robot arm, X1 and Z1 are the X axis and the Z axis of the coordinate system of the first arm joint, X2 and Z2 are the X axis and the Z axis of the coordinate system of the second arm joint, X3 and Z3 are the X axis and the Z axis of the coordinate system of the third arm joint, X4 and Z4 are the X axis and the Z axis of the coordinate system of the fourth arm joint, and X5 and Z5 are the fifth arm jointX6, Z6 are the X-axis and Z-axis of the coordinate system of the sixth boom joint, X7, Z7 are the X-axis and Z-axis of the coordinate system of the seventh boom joint, and X8, Y8, Z8 are the X-axis, Y-axis and Z-axis of the coordinate system of the eighth boom joint. Further, each parameter in the coordinate system of the multiple arm support joints of the redundant mechanical arm is as follows: a1Represents the deviation of the coordinate system of the first arm support and the second arm support joint in the X-axis direction, a2A deviation of the Z axis of the coordinate system representing the joint of the second arm and the third arm in the direction of the X axis, -a4A deviation of the Z-axis of the coordinate system representing the joint of the fourth boom and the fifth boom in the X-axis direction, -a5A Z-axis offset in the X-axis direction of a coordinate system representing the fifth arm support and the sixth arm support joint, d3A deviation of the X-axis of the coordinate system of the second arm support and the third arm support joint in the Z-axis direction, d6A deviation of the X-axis of the coordinate system of the fifth arm support and the sixth arm support joint in the Z-axis direction, d7The offset of the X axis of the coordinate system of the sixth arm support and the seventh arm support joint in the Z axis direction is shown, d8And L4 represents the offset of the coordinate system of the seventh arm support and the eighth arm support joint in the Z-axis direction, and the telescopic distance of the sliding pair.
Further, θ will beiThe included angle of the coordinate systems of the adjacent two arm support joints around the Z axis is also the corner of the arm support joint, diRepresents the deviation of X-axis of the joint coordinate systems of two adjacent arm frames in the Z-axis direction, ai-1Represents the deviation of Z axis of the joint coordinate systems of two adjacent arm frames in the X axis direction, alphai-1Representing the included angle of the Z axis of the joint coordinate systems of the two adjacent arm frames around the X axis;
then, a DH method is used to construct mathematical models of multiple boom joints of the spatial redundant manipulator, and the mathematical models are established with the poses shown in the coordinate system of the redundant manipulator in fig. 2 as initial poses, as shown in table 1:
table 1: mathematical model of multiple arm support joints of redundant mechanical arm
i θi di ai-1 αi-1
1 0 0 0 0
2 -π/2 0 -a1 -π/2
3 0 d3 a2 0
4 0 L4 0 -π/2
5 0 0 -a4 π/2
6 0 d6 -a5 -π/2
7 -π/2 d7 0 -π/2
8 0 d 8 0 0
Wherein, i represents the joints of two adjacent arm frames, and when i takes 1 to 8, the joints between the first arm frame and the base, the first arm frame and the second arm frame, the second arm frame and the third arm frame, the third arm frame and the fourth arm frame, the fourth arm frame and the fifth arm frame, the fifth arm frame and the sixth arm frame, the sixth arm frame and the seventh arm frame, and the seventh arm frame and the eighth arm frame respectively represent, that is, the joints of the arm frames are represented by i. The included angle between any two adjacent arm supports of the redundant mechanical arm changes in the motion process, and the initial state position of the redundant mechanical arm is shown in fig. 2.
Further, when the mechanical arm is a fixed-rod-length constraint mechanical arm, the fixed-rod-length constraint mechanical arm shown in fig. 3 is taken as an exemplary illustration, and the fixed-rod-length constraint mechanical arm includes six revolute pairs and has six self-contained spacesIn the coordinate system of the rotating base of the fixed-rod-length constraint mechanical arm and the plurality of arm support joints shown in fig. 3, the fixed-rod-length constraint mechanical arm locks the moving pair in the redundant mechanical arm, so that the redundant mechanical arm has six rotating pairs and one arm support joint is less in the locked condition. In fig. 3, X0 and Z0 are the X axis and Z axis of the coordinate system (i.e. base coordinate system) of the swing base of the robot arm, X1 and Z1 are the X axis and Z axis of the coordinate system of the first arm joint, X2 and Z2 are the X axis and Z axis of the coordinate system of the second arm joint, X3 and Z3 are the X axis and Z axis of the coordinate system of the third arm joint, X4 and Z4 are the X axis and Z axis of the coordinate system of the fourth arm joint, X5 and Z5 are the X axis and Z axis of the coordinate system of the fifth arm joint, X6 and Z6 are the X axis and Z axis of the coordinate system of the sixth arm joint, and X7, Y7 and Z7 are the X axis, Y axis and Z axis of the coordinate system of the seventh arm joint. Further, the parameters in the coordinate system of the fixed rod length constraint mechanical arm multiple arm support joints are respectively as follows: a1A deviation of the Z axis of the coordinate system of the first arm support and the second arm support joint in the X axis direction, a2The offset of the Z axis of the coordinate system of the second arm support and the third arm support joint in the X axis direction is shown, a3The offset of the Z axis of the coordinate system of the third arm support and the fourth arm support joint in the X axis direction is shown, a4A Z-axis offset in the X-axis direction of a coordinate system representing the joints of the fourth boom and the fifth boom, d3A deviation of the X-axis of the coordinate system of the second arm support and the third arm support joint in the Z-axis direction, d5The offset of the X axis of the coordinate system of the fourth arm support and the fifth arm support joint in the Z axis direction, d7And L4 represents the telescopic distance of the moving pair, and the telescopic distance of the moving pair in the fixed rod length constraint mechanical arm is always 0.
Further, θ will beiRepresenting the included angle around the Z axis of the coordinate systems of the joints of two adjacent arm frames, diRepresents the deviation of X-axis of the joint coordinate systems of two adjacent arm frames in the Z-axis direction, ai-1Represents the deviation of Z axis of the joint coordinate systems of two adjacent arm frames in the X axis direction, alphai-1Representing the coordinate system of the joints of two adjacent arm framesThe Z axis of the Z-axis is around the included angle of the X axis;
then, a DH method is used to construct mathematical models of multiple arm support joints of the spatial fixed-rod-length constraint mechanical arm, and the mathematical models are established with the pose shown in the coordinate system of the fixed-rod-length constraint mechanical arm in fig. 3 as an initial pose, as shown in table 2:
table 2: mathematical model for fixing rod length and constraining joints of multiple arm supports of mechanical arm
i θi di ai-1 αi-1
1 0 0 0 0
2 -π/2 0 -a1 -π/2
3 0 d3 a2 0
4 0 0 a3 0
5 0 d5 a4 π/2
6 0 d6 0 -π/2
7 0 d 7 0 0
Wherein i represents two adjacent arm support joints, when i takes 1-7, the two adjacent arm support joints represented respectively are a first arm support and a rotary base, the first arm support and a second arm support, the second arm support and a third arm support, the third arm support and a fourth arm support, the fourth arm support and a fifth arm support, the fifth arm support and a sixth arm support, and the joint between the sixth arm support and a seventh arm support, namely the arm support joint represented by i.
In the embodiment, the inverse kinematics solution algorithm for constructing the mechanical arm at the control layer based on the mathematical models of the mechanical arm rotary base and the plurality of arm support joints and the use condition of the mechanical arm comprises,
acquiring a motion direction and a speed signal of the tail end of the mechanical arm from a current point to a target point;
based on a coordinate system and a mathematical model of a mechanical arm rotary base and a plurality of arm support joints and the motion direction and speed signals, calculating current position and posture information of the tail end of the mechanical arm, and calculating a track function from a current point to a target point of the tail end of the mechanical arm;
and calculating the track function corresponding to each arm support joint in the motion process according to inverse kinematics based on the track function from the current point to the target point at the tail end of the mechanical arm.
In this embodiment, the operating condition of the mechanical arm includes a CP motion mode with a fixed trajectory parameter and a mechanical arm end motion control signal. The CP (Continuous Path) motion mode of the fixed track parameter refers to a procedure for setting the motion track of the mechanical arm to be fixed, and the mechanical arm only moves repeatedly on the set track. The mechanical arm end motion control comprises a mechanical arm motion starting stop signal or a motion direction and speed signal which is given to the mechanical arm end from a current point to a target point.
The inverse kinematics solution algorithm comprises a redundant degree of freedom numerical solution and a spatial six-degree of freedom analytical solution.
Specifically, the building of the inverse kinematics solution algorithm of the mechanical arm at the control layer based on the mathematical models of the joints of the multiple arm frames and the use conditions of the mechanical arm comprises building a redundant degree of freedom numerical solution, wherein the building comprises setting the pose of the tail end of the redundant mechanical arm to (x, y, Z, alpha, beta, gamma), wherein (x, y, Z) represents the position of the tail end of the mechanical arm in space, and (alpha, beta, gamma) represents the pose of the tail end of the mechanical arm in space, and the included angles of the joint coordinate systems of two adjacent arm frames from a rotary base to a third arm frame and from a fourth arm frame to a seventh arm frame in the redundant mechanical arm around the Z axis are respectively theta1,θ2,θ3,θ5,θ6,θ7And the cylinder body elongation of each driving cylinder in the redundant mechanical arm is l2,l3,l4,l5,l6Specifically, in the embodiment of the present invention, θ4Always 0, l2,l3,l4,l5,l6The extension of the cylinder body of the driving cylinder between the first arm support and the second arm support, the extension of the cylinder body of the driving cylinder between the second arm support and the third arm support, the extension of the cylinder body of the driving cylinder between the third arm support and the fourth arm support, the extension of the cylinder body of the driving cylinder between the fourth arm support and the fifth arm support, and the extension of the cylinder body of the driving cylinder between the fifth arm support and the sixth arm support are represented respectively, and the extension of the cylinder body of the driving cylinder between the third arm support and the fourth arm support is the extension of the cylinder body of the moving pair driving cylinder.
Then, the following functional relationships are respectively determined:
pose and theta of the end of the redundant manipulator1,θ2,θ3,θ5,θ6,θ7And l4Functional relationship of (1), theta in redundant robot arm2,θ3,θ5,θ6Are respectively corresponding to2,l3,l5,l6Functional relation between the end speed of the redundant mechanical arm and the angular speed of the joint from the first arm frame joint to the seventh arm frame joint in the redundant mechanical arm, and the redundant mechanical arm
Figure GDA0003299672220000156
And
Figure GDA0003299672220000151
in a functional relationship therebetween, wherein,
Figure GDA0003299672220000157
the telescopic speed of a driving cylinder between a first arm support and a second arm support, the telescopic speed of a driving cylinder between the second arm support and a third arm support, the telescopic speed of a driving cylinder between a fourth arm support and a fifth arm support, and the telescopic speed of a driving cylinder between the fifth arm support and a sixth arm support;
Figure GDA0003299672220000152
second oneThe joint angular velocities of the boom joint, the third boom joint, the fifth boom joint, and the sixth boom joint.
Specifically, the pose and θ of the end of the redundant robot arm1,θ2,θ3,θ5,θ6,θ7And l4The functional relationship of (A) is as follows:
x＝f(θ1,θ2,θ3,l4,θ5,θ6,θ7)
y＝f(θ1,θ2,θ3,l4,θ5,θ6,θ7)
z＝f(θ1,θ2,θ3,l4,θ5,θ6,θ7)
α＝f(θ1,θ2,θ3,l4,θ5,θ6,θ7)
β＝f(θ1,θ2,θ3,l4,θ5,θ6,θ7)
γ＝f(θ1,θ2,θ3,l4,θ5,θ6,θ7)
theta in the redundant robot arm2,θ3,θ5,θ6Are respectively corresponding to2,l3,l5,l6Functional relationship between:
θ2＝g(l2)
θ3＝g(l3)
θ5＝g(l5)
θ6＝g(l6)
the functional relation between the speed of the tail end of the redundant mechanical arm and the angular speeds of the joints from the first arm frame joint to the seventh arm frame joint in the redundant mechanical arm is as follows:
Figure GDA0003299672220000153
wherein the content of the first and second substances,
Figure GDA0003299672220000154
respectively represents the velocity components of the redundant mechanical arm tail end velocity in the pose,
Figure GDA0003299672220000155
representing the joint angular velocity, f, of the first boom joint to the seventh boom jointx、fy、fz(omitted from the formula), fα(omitted from the formula), fβ(omitted from the formula), fγAnd representing dependent variables of all parameters in the pose.
In the redundant mechanical arm
Figure GDA0003299672220000168
And
Figure GDA0003299672220000161
functional relationship between:
Figure GDA0003299672220000162
Figure GDA0003299672220000163
Figure GDA0003299672220000164
Figure GDA0003299672220000165
wherein the content of the first and second substances,
Figure GDA0003299672220000169
the telescopic speed of the driving cylinder between the first arm support and the second arm support and the driving cylinder between the second arm support and the third arm supportThe telescopic speed of the driving cylinder between the fourth arm support and the fifth arm support, and the telescopic speed of the driving cylinder between the fifth arm support and the sixth arm support;
Figure GDA0003299672220000166
Figure GDA0003299672220000167
joint angular velocities of the second boom joint, the third boom joint, the fifth boom joint, and the sixth boom joint.
The redundant degree of freedom exists in the mechanical arm, multiple different corresponding relations exist between the terminal pose and the joint angle and the elongation, and when an obstacle exists, the redundant mechanical arm can conduct self-adaptive adjustment of the terminal position and the terminal pose through the redundant degree of freedom to achieve obstacle avoidance, so that the redundant mechanical arm can adapt to the external environment better.
Further, when constructing a spatial six-degree-of-freedom analytical solution, the method comprises the steps of firstly, obtaining the pose (p) of the tail end of the fixed rod length constraint mechanical armx,py,pzα, β, γ), wherein (p)x,py,pz) The position of the tail end of the fixed rod length constraint mechanical arm in the space is shown, (alpha, beta, gamma) shows the posture of the tail end of the fixed rod length constraint mechanical arm in the space, and the included angles of joint coordinate systems of two adjacent arm frames around the Z axis from a first arm frame to a fourth arm frame in the fixed rod length constraint mechanical arm are theta2,θ3,θ4And the extension amounts of cylinder bodies of the driving cylinders between the first arm support and the fourth arm support in the fixed rod length constraint mechanical arm are respectively l2,l3,l4Specifically, l2,l3,l4Respectively representing the cylinder body elongation of a driving cylinder between the first arm support and the second arm support, the cylinder body elongation of a driving cylinder between the second arm support and the third arm support, and the cylinder body elongation of a driving cylinder between the third arm support and the fourth arm support;
secondly, two representation modes are adopted to represent the pose of the tail end of the mechanical arm, wherein the first representation mode is as follows: at any moment, the pose of the tail end of the fixed rod length constraint mechanical arm is expressed as a matrix T under a base coordinate system:
Figure GDA0003299672220000171
wherein n isx、ny、nz、ox、oy、oz、ax、ay、azFor representing attitude parameters, p, of the pose of the end of the fixed-bar length constrained manipulatorx、py、pzThe position parameter is used for representing the position of the fixed rod length constraint mechanical arm end in the space.
The second expression mode is as follows: based on a fixed rod length constraint mechanical arm mathematical model, using theta for each item in the matrix TiRepresenting, obtaining, matrix Tθ：
Figure GDA0003299672220000172
Then, based on the first representation mode and the second representation mode, determining an included angle theta of the coordinate system of two adjacent arm support joints of the fixed-rod-length constraint mechanical arm from the first arm support to the sixth arm support joint around the Z axis1,θ2,θ3,θ4,θ5,θ6The kinematic analysis expression of (1), wherein the value range of i is the value of i in the corresponding mathematical model table, specifically comprising the following steps:
step 1, based on the deviation d of the X axis of the coordinate system of the first arm support and the second arm support joint and the second arm support and the third arm support joint in the Z axis direction2、d3Making the matrix elements (2,4) correspondingly equal in the two expression modes, and solving the included angle theta between the first arm support joint and the rotary base around the Z axis1The kinematic analytical expression of (a):
Figure GDA0003299672220000173
where the matrix elements (2,4) represent the elements in row 2 and column 4 of the matrix. Atan stands for arctan.
Step 2, correspondingly equalizing the matrix elements (2,3) in the two expression modes, and solving an included angle theta between the joint coordinate systems of the fourth arm support and the fifth arm support around the Z axis5The kinematic analytical expression of (a):
θ5＝±arccos(axs1-ayc1)
where the matrix elements (2,3) represent the elements in row 2 and column 3 of the matrix. c. C1Represents cos (theta)1)，s1Represents sin (theta)1)。
Step 3, correspondingly equalizing the matrix elements (2,2) in the two expression modes, and solving an included angle theta of a fifth arm support and a sixth arm support joint coordinate system around the Z axis6The kinematic analytical expression of (a):
Figure GDA0003299672220000181
where matrix elements (2,2) represent the elements in row 2 and column 2 of the matrix. c. C1Represents cos (theta)1)，s1Represents sin (theta)1)。
Step 4, based on the coordinate system of the second arm support and the third arm support joint and the deviation a of the Z axis of the coordinate system of the third arm support and the fourth arm support joint in the X axis direction2、a3Respectively and correspondingly equalizing the matrix elements (1,4) and (3,4) in the two expression modes, unfolding and shifting the intermediate items, and sequentially obtaining the included angle theta of the coordinate systems of the first arm support and the second arm support joint and the second arm support and the third arm support joint around the Z axis2、θ3The kinematic analytical expression of (a):
Figure GDA0003299672220000182
θ2＝A tan 2(s2，c2)
wherein the matrix elements (1,4) represent the 1 st row in the matrix4 columns of elements, the matrix elements (3,4) representing the elements of row 3 and column 4 in the matrix, c2Represents cos (theta)2)，s2Represents sin (theta)2)。
Step 5, based on the included angle theta of the coordinate systems of the first arm support and the second arm support joint and the second arm support and the third arm support joint around the Z axis4Respectively and correspondingly equalizing matrix elements (1,3) and (3,3) in the two expression modes, unfolding and shifting the intermediate items, and solving an included angle theta of a joint coordinate system of the third arm support and the fourth arm support around the Z axis4The kinematic analytical expression of (a):
θ4＝Atan2(-s6(nxc1+nys1)-c6(oxc1+oys1)，ozc6+nzs6)-θ2-θ3
wherein the matrix elements (1,3) represent the elements of the 1 st row and 3 rd column in the matrix, the matrix elements (3,3) represent the elements of the 3 rd row and 3 rd column in the matrix, c1Represents cos (theta)1)，s1Represents sin (theta)1)，c6Represents cos (theta)6)，s6Represents sin (theta)6)。
Then, the following functional relationships are respectively determined:
theta in fixed rod length constraint mechanical arm2,θ3,θ4Are respectively corresponding to2,l3,l4Functional relation between the fixed rod length and the fixed rod length, functional relation between the tail end speed of the fixed rod length constraint mechanical arm and the joint angular speed from the first arm frame joint to the seventh arm frame joint in the fixed rod length constraint mechanical arm, and functional relation between the fixed rod length and the fixed rod length constraint mechanical arm
Figure GDA0003299672220000194
And
Figure GDA0003299672220000191
in a functional relationship between (a) and (b),
Figure GDA0003299672220000195
the extension amount of a cylinder body of a driving cylinder between a first arm support and a second arm support, the extension amount of a cylinder body of a driving cylinder between the second arm support and a third arm support, the extension amount of a cylinder body of a driving cylinder between a fourth arm support and a fifth arm support, and the extension speed of a driving cylinder between the fifth arm support and a sixth arm support are measured;
Figure GDA0003299672220000192
joint angular velocities of the second boom joint, the third boom joint, and the fourth boom joint.
Specifically, the fixed rod length restrains theta in the mechanical arm2,θ3,θ4Are respectively corresponding to2,l3,l4Functional relationship between:
θ2＝g(l2)
θ3＝g(l3)
θ4＝g(l4)
the function relation between the speed of the tail end of the fixed rod length constraint mechanical arm and the angular speeds of the joints from the first arm frame joint to the seventh arm frame joint in the fixed rod length constraint mechanical arm is as follows:
Figure GDA0003299672220000193
wherein J is a Jacobian matrix represented by the constraint mechanical arm with fixed rod length under a base coordinate system,
Figure GDA0003299672220000201
representing the velocity component of the redundant manipulator tip velocity in the pose,
Figure GDA0003299672220000202
the joint angular velocities of the first boom joint to the seventh boom joint are represented.
In the fixed rod length constraint mechanical arm
Figure GDA0003299672220000208
And
Figure GDA0003299672220000203
functional relationship between:
Figure GDA0003299672220000204
Figure GDA0003299672220000205
Figure GDA0003299672220000206
wherein the content of the first and second substances,
Figure GDA0003299672220000209
the telescopic speed of a driving cylinder between the first arm support and the second arm support, the telescopic speed of a driving cylinder between the second arm support and the third arm support and the telescopic speed of a driving cylinder between the third arm support and the fourth arm support are set;
Figure GDA0003299672220000207
joint angular velocities of the second boom joint, the third boom joint, and the fourth boom joint.
In this embodiment, planning a trajectory function of each arm support joint of the mechanical arm at a planning layer includes, first, acquiring a motion direction and a velocity signal of a terminal of the mechanical arm from a current point to a target point; secondly, calculating the current position and posture information of the tail end of the mechanical arm based on a mechanical arm fixing base coordinate system, a coordinate system and a mathematical model of a plurality of arm support joints, joint signals and control instructions; then, calculating a track function from the current point to the target point of the tail end of the mechanical arm based on the control instruction; and finally, calculating a track function corresponding to each arm support joint in the motion process of the mechanical arm based on a kinematics inverse solution algorithm. Further, a simulation mode may be adopted in the planning layer to obtain a trajectory function corresponding to each arm support joint, as shown in fig. 6, the obtained trajectory function of each arm support joint in the process of linear motion of each arm support joint along the X-axis direction is taken as an example of the redundant mechanical arm shown in fig. 2. When the control method provided by the invention is applied to multi-joint linkage intelligent control of the spatial redundancy mechanical arm, the continuous and smooth motion of the tail end of the mechanical arm and the joint angle of each arm support can be realized. Preferably, the track function is a track function of the angle value of each arm support joint
In this embodiment, as shown in fig. 5, the control method further includes that, first, the mechanical arm receives a control instruction sent by the control device; then, judging whether the configuration of the mechanical arm is a redundant mechanical arm or not, wherein,
when the configuration of the mechanical arm is a redundant mechanical arm, selecting a redundant degree of freedom numerical solution at a control layer;
and when the configuration of the mechanical arm is a fixed rod length constraint mechanical arm, selecting a spatial six-degree-of-freedom analytic solution at the control layer.
In the embodiment, when redundant degrees of freedom exist in the motion process of the mechanical arm, the extension range of the joint can be restrained by adding restraint conditions to the length of each arm support joint telescopic cylinder or the joint corner, meanwhile, in the closed-loop control process, relevant parameters can be obtained according to the type of an actual driving device, threshold setting is carried out on the mechanical arm, if the driving device is in the telescopic cylinder form, the safety threshold of the obtained internal pressure value of the joint telescopic cylinder body is set according to the use working condition, in the motion process of the mechanical arm, if the internal pressure of the cylinder body exceeds the safety threshold, the mechanical arm stops moving, and at the moment, the mechanical arm can be adjusted to the safety pose through manual single joint motion or temporary incapability of the pressure protection function. The constraint conditions can comprise the maximum and minimum ranges of the extension amount of the telescopic cylinder on the mechanical arm, the extension amount of the telescopic cylinder obtained by a kinematics inverse solution algorithm and a plurality of mathematical models of arm support joints is firstly compared with the constraint conditions, and if the value calculated by the algorithm is in the telescopic range, the value calculated by the algorithm can be used as a control command; if the value calculated by the algorithm is not in the range, the fact that the actual physical condition cannot be realized although the mathematical model has a solution is shown, and the range of the telescopic cylinder is exceeded.
The embodiment of the invention is a spatial redundant mechanical arm, but is not limited to the configuration, and the control algorithm can be customized and adjusted according to the characteristics of the arm support arrangement mode, the degree of freedom and the like of the controlled object, so that the control requirements of different control objects such as whether offset exists, whether a moving pair exists, whether redundant degree of freedom exists and the like are met, for example, an excavator, a concrete pump truck, an engineering mechanical arm with a multi-degree-of-freedom tail gripper and the like; and finally, flexibly selecting the control system used by the control method or deleting the designed related signal acquisition equipment according to the control precision requirement of the engineering mechanical arm.
Although the present invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; and such modifications or substitutions do not depart from the spirit and scope of the corresponding technical solutions of the embodiments of the present invention.

Claims (5)

1. A multi-joint linkage control method of a mechanical arm is characterized by comprising the following steps,
constructing a mechanical arm control system frame, wherein the control system frame comprises a control layer, a model layer and a planning layer;
constructing a base coordinate system fixed by the mechanical arm, coordinate systems of a plurality of arm support joints and mathematical models of the plurality of arm support joints on the model layer;
constructing a kinematics inverse solution algorithm of the mechanical arm at the control layer based on the mathematical models of the joints of the plurality of arm supports and the use condition of the mechanical arm;
planning a track function of each arm support joint of the mechanical arm on a planning layer based on a kinematics inverse solution algorithm;
constructing a base coordinate system fixed by the mechanical arm, a coordinate system of a plurality of arm support joints and a mathematical model of the plurality of arm support joints on a model layer,
establishing a fixed base coordinate system at a rotary base of the mechanical arm;
determining the mechanical arm configuration of the mechanical arm, and acquiring the elongation and joint angle of each arm support joint in a plurality of arm support joints in the mechanical arm;
establishing a coordinate system of a plurality of arm support joints based on a base coordinate system, the configuration of the mechanical arm, the elongation and the joint corner of each arm support joint, and establishing a mathematical model of the plurality of arm support joints by using a DH method;
the mechanical arm configuration comprises a redundant mechanical arm and a fixed rod length constraint mechanical arm, wherein,
when the mechanical arm is a redundant mechanical arm, the redundant mechanical arm comprises six revolute pairs and one revolute pair, six spatial degrees of freedom are provided, and the coordinate systems of the joints of the arm frames meet the following requirements:
the coordinate systems of the first arm support joint to the seventh arm support joint comprise an X axis and a Z axis, and the coordinate system of the eighth arm support joint comprises an X axis, a Y axis and a Z axis;
the mathematical models of the arm support joints satisfy that:
in an initial state, the included angle of the coordinate systems of any two adjacent arm support joints around the Z axis is 0;
the offset of the X axis of the coordinate system of the third arm support joint in the Z axis direction of the coordinate system of the second arm support joint is d3The offset of the X axis of the coordinate system of the fifth arm support joint in the Z axis direction of the coordinate system of the fourth arm support joint is d6The offset of the X axis of the coordinate system of the seventh boom joint in the Z axis direction of the coordinate system of the sixth boom joint is d7The offset of the X axis of the coordinate system of the eighth boom joint in the Z axis direction of the coordinate system of the seventh boom joint is d8；
The offset of the Z axis of the coordinate system of the second arm support joint in the X axis direction of the coordinate system of the first arm support joint is-a1The offset of the Z axis of the coordinate system of the third arm support joint in the X axis direction of the coordinate system of the second arm support joint is a2The offset of the Z axis of the coordinate system of the fifth arm support joint in the X axis direction of the coordinate system of the fourth arm support joint is-a4The offset of the Z axis of the coordinate system of the sixth arm support joint in the X axis direction of the coordinate system of the fifth arm support joint is-a5；
The telescopic distance of the sliding pair is L4;
when the mechanical arm configuration is a fixed rod length constraint mechanical arm, the fixed rod length constraint mechanical arm comprises six revolute pairs and has six spatial degrees of freedom, and the coordinate systems of the joints of the arm frames meet the following requirements:
the coordinate systems of the first arm support joint to the sixth arm support joint comprise an X axis and a Z axis, and the coordinate system of the seventh arm support joint comprises an X axis, a Y axis and a Z axis;
the mathematical models of the arm support joints satisfy that:
in an initial state, the included angle of the coordinate systems of any two adjacent arm frame joints around the Z axis is 0;
the offset of the X axis of the coordinate system of the third arm support joint in the Z axis direction of the coordinate system of the second arm support joint is d3The offset of the X axis of the coordinate system of the fifth arm support joint in the Z axis direction of the coordinate system of the fourth arm support joint is d5The offset of the X axis of the coordinate system of the sixth arm support joint in the Z axis direction of the coordinate system of the fifth arm support joint is d6The offset of the X axis of the coordinate system of the seventh boom joint in the Z axis direction of the coordinate system of the sixth boom joint is d7；
The offset of the Z axis of the coordinate system of the second arm support joint in the X axis direction of the coordinate system of the first arm support joint is-a1The offset of the Z axis of the coordinate system of the third arm support joint in the X axis direction of the coordinate system of the second arm support joint is a2The offset of the Z axis of the coordinate system of the fourth boom joint in the X axis direction of the coordinate system of the third boom joint is a3The offset of the Z axis of the coordinate system of the fifth arm support joint in the X axis direction of the coordinate system of the fourth arm support joint is a4(ii) a When the robotic arm is configured as a fixed-bar length constrained robotic arm, wherein,
based on the mathematical models of a plurality of arm support joints and the use condition of the mechanical arm, the kinematics inverse solution algorithm for constructing the mechanical arm at the control layer comprises the steps of constructing a spatial six-degree-of-freedom analytic solution, wherein the method comprises the following steps,
set the fixed rod length to restrain the end of the mechanical arm based on the mathematical model of the arm support jointsPosition and posture of (p)x,py,pzα, β, γ), wherein (p)x,py,pz) The terminal position of the tail end of the fixed rod length constraint mechanical arm in the space is shown, (alpha, beta, gamma) shows the posture of the tail end of the fixed rod length constraint mechanical arm in the space, and the included angles of the coordinate systems of two adjacent arm frame joints from the first arm frame joint to the fourth arm frame joint in the fixed rod length constraint mechanical arm around the Z axis are respectively theta2,θ3,θ4And the extension amounts of cylinder bodies of driving cylinders between a first arm support joint and a fourth arm support joint in the fixed rod length constraint mechanical arm are respectively l2,l3,l4；
Two representation modes are adopted to represent the pose of the tail end of the mechanical arm, wherein,
the first expression is as follows: at any moment, the pose of the tail end of the fixed rod length constraint mechanical arm is expressed as a matrix T under a base coordinate system:
Figure FDA0003373557780000031
wherein n isx、ny、nz、ox、oy、oz、ax、ay、azRepresenting attitude parameters of the tail end pose of the fixed rod length constraint mechanical arm;
the second expression mode is as follows: based on a fixed rod length constraint mechanical arm mathematical model, using theta for each item in the matrix TiRepresenting, obtaining, matrix Tθ：
Figure FDA0003373557780000041
Determining the included angle theta of the coordinate system of two adjacent arm support joints of the fixed rod length constraint mechanical arm from the first arm support joint to the sixth arm support joint around the Z axis based on the first representation mode and the second representation mode1,θ2,θ3,θ4,θ5,θ6The kinematic analysis expression of (a);
the following functional relationships are respectively determined:
theta in fixed rod length constraint mechanical arm2,θ3,θ4Are respectively corresponding to2,l3,l4Functional relation between the fixed rod length and the fixed rod length, functional relation between the tail end speed of the fixed rod length constraint mechanical arm and the joint angular speed from the first arm frame joint to the seventh arm frame joint in the fixed rod length constraint mechanical arm, and functional relation between the fixed rod length and the fixed rod length constraint mechanical arm
Figure FDA0003373557780000042
And
Figure FDA0003373557780000043
in a functional relationship therebetween, wherein,
Figure FDA0003373557780000044
the telescopic speed of the driving cylinder between the first arm support joint and the second arm support joint, the telescopic speed of the driving cylinder between the second arm support joint and the third arm support joint and the telescopic speed of the driving cylinder between the third arm support joint and the fourth arm support joint are respectively set;
Figure FDA0003373557780000045
the joint angular velocities of the second arm support joint, the third arm support joint and the fourth arm support joint are respectively;
determining an included angle theta of a coordinate system of two adjacent arm support joints of the fixed rod length constraint mechanical arm from the first arm support joint to the sixth arm support joint around the Z axis based on the first representation mode and the second representation mode1,θ2,θ3,θ4,θ5,θ6The kinematic analysis expression specifically comprises the following steps:
step 1, the deviation of the X axis of the coordinate system of the second arm support joint in the Z axis direction of the coordinate system of the first arm support joint is d2The offset of the X axis of the coordinate system of the third arm support joint in the Z axis direction of the coordinate system of the second arm support joint is d3Let twoThe matrix elements (2,4) are correspondingly equal in the representation mode, and the included angle theta between the first arm support joint and the rotary base around the Z axis is obtained1The kinematic analytical expression of (a):
Figure FDA0003373557780000051
wherein, the matrix elements (2,4) represent the elements of the 2 nd row and the 4 th column in the matrix, and Atan represents the arctangent;
step 2, correspondingly equalizing the matrix elements (2,3) in the two expression modes, and solving an included angle theta between the coordinate system of the fourth arm support joint and the coordinate system of the fifth arm support joint around the Z axis5The kinematic analytical expression of (a):
θ5＝±arccos(axs1-ayc1)
where the matrix elements (2,3) represent the elements in row 2 and column 3 of the matrix, c1Represents cos θ1，s1Represents sin θ1；
Step 3, correspondingly equalizing the matrix elements (2,2) in the two expression modes, and solving an included angle theta between the coordinate system of the fifth arm support joint and the coordinate system of the sixth arm support joint around the Z axis6The kinematic analytical expression of (a):
Figure FDA0003373557780000052
where the matrix elements (2,2) represent the elements of row 2 and column 2 in the matrix, c1Represents cos θ1，s1Represents sin θ1Atan represents the arctan;
step 4, respectively and correspondingly equalizing the matrix elements (1,4) and (3,4) in the two expression modes, expanding and shifting the intermediate items, and then calculating theta2、θ3The kinematic analytical expression of (a):
Figure FDA0003373557780000053
θ2＝Atan2(s2，c2)
wherein the matrix elements (1,4) represent the elements of the 1 st row and 4 th column of the matrix, the matrix elements (3,4) represent the elements of the 3 rd row and 4 th column of the matrix, c2Represents cos θ2，s2Represents sin θ2；
Step 5, respectively corresponding and equaling the matrix elements (1,3) and (3,3) under the two expression modes, and expanding and shifting the intermediate term to obtain theta4The kinematic analytical expression of (a):
θ4＝Atan2(-s6(nxc1+nys1)-c6(oxc1+oys1)，ozc6+nzs6)-θ2-θ3
wherein the matrix elements (1,3) represent the elements of the 1 st row and 3 rd column in the matrix, the matrix elements (3,3) represent the elements of the 3 rd row and 3 rd column in the matrix, c1Represents cos θ1，s1Represents sin θ1，c6Represents cos θ6，s6Represents sin θ6；
Theta in the fixed rod length constraint mechanical arm2,θ3,θ4Are respectively corresponding to2,l3,l4Functional relationship between:
θ2＝g(l2)
θ3＝g(l3)
θ4＝g(l4)
the function relation between the speed of the tail end of the fixed rod length constraint mechanical arm and the angular speeds of the joints from the first arm frame joint to the seventh arm frame joint in the fixed rod length constraint mechanical arm is as follows:
Figure FDA0003373557780000061
wherein J is a Jacobian matrix represented by the constraint mechanical arm with fixed rod length under a base coordinate system,
Figure FDA0003373557780000062
representing the velocity component of the fixed rod length constraint mechanical arm tail end velocity in the pose,
Figure FDA0003373557780000063
representing joint angular velocities of the first arm support joint to the seventh arm support joint;
in the fixed rod length constraint mechanical arm
Figure FDA0003373557780000064
And
Figure FDA0003373557780000065
functional relationship between:
Figure FDA0003373557780000071
2. the multi-joint linkage control method of the mechanical arm according to claim 1, wherein when the mechanical arm is a redundant mechanical arm, the method is characterized in that the inverse kinematics solution algorithm for constructing the mechanical arm at the control layer based on the mathematical models of the joints of the plurality of arm supports and the using condition of the mechanical arm comprises constructing a redundant degree of freedom numerical solution, wherein the method comprises the following steps,
setting the pose of the tail end of the redundant mechanical arm as (x, y, Z, alpha, beta, gamma) based on a mathematical model of a plurality of arm frame joints, wherein (x, y, Z) represents the position of the tail end of the mechanical arm in space, and (alpha, beta, gamma) represents the posture of the tail end of the mechanical arm in space, and the included angles of coordinate systems of two adjacent arm frame joints from a rotary base to a third arm frame joint and from a fourth arm frame joint to a seventh arm frame joint around a Z axis in the redundant mechanical arm are respectively theta1,θ2,θ3,θ5,θ6,θ7And the cylinder body elongation of each driving cylinder in the redundant mechanical arm is l2,l3,l4,l5,l6；
The following functional relationships are respectively determined:
pose and theta of the end of the redundant manipulator1,θ2,θ3,θ5,θ6,θ7And l4Functional relationship of (1), theta in redundant robot arm2,θ3,θ5,θ6Are respectively corresponding to2,l3,l5,l6Functional relation between the end speed of the redundant mechanical arm and the angular speed of the joint from the first arm frame joint to the seventh arm frame joint in the redundant mechanical arm, and the redundant mechanical arm
Figure FDA0003373557780000072
And
Figure FDA0003373557780000073
in a functional relationship therebetween, wherein,
Figure FDA0003373557780000074
respectively setting the stretching speed of a driving cylinder between a first arm support joint and a second arm support joint, the stretching speed of a driving cylinder between the second arm support joint and a third arm support joint, the stretching speed of a driving cylinder between a fourth arm support joint and a fifth arm support joint, and the stretching speed of a driving cylinder between the fifth arm support joint and a sixth arm support joint;
Figure FDA0003373557780000075
the joint angular velocities of the second arm support joint, the third arm support joint, the fifth arm support joint and the sixth arm support joint are respectively.
3. The multi-joint linkage control method for a robot arm according to claim 2,
pose and pose of the end of the redundant mechanical armθ1,θ2,θ3,θ5,θ6,θ7And l4Functional relationship of (a):
x＝f(θ1,θ2,θ3,l4,θ5,θ6,θ7)
y＝f(θ1,θ2,θ3,l4,θ5,θ6,θ7)
z＝f(θ1,θ2,θ3,l4,θ5,θ6,θ7)
α＝f(θ1,θ2,θ3,l4,θ5,θ6,θ7)
β＝f(θ1,θ2,θ3,l4,θ5,θ6,θ7)
γ＝f(θ1,θ2,θ3,l4,θ5,θ6,θ7)；
theta in the redundant robot arm2,θ3,θ5,θ6Are respectively corresponding to2,l3,l5,l6Functional relationship between:
θ2＝g(l2)
θ3＝g(l3)
θ5＝g(l5)
θ6＝g(l6)；
the functional relation between the speed of the tail end of the redundant mechanical arm and the angular speeds of the joints from the first arm frame joint to the seventh arm frame joint in the redundant mechanical arm is as follows:
Figure FDA0003373557780000081
wherein the content of the first and second substances,
Figure FDA0003373557780000082
representing the velocity component of the redundant manipulator tip velocity in the pose,
Figure FDA0003373557780000083
representing the joint angular velocity, f, of the first boom joint to the seventh boom jointx、fy、fγRepresenting dependent variables of all parameters in the pose;
in the redundant mechanical arm
Figure FDA0003373557780000084
And
Figure FDA0003373557780000085
functional relationship between:
Figure FDA0003373557780000091
Figure FDA0003373557780000092
Figure FDA0003373557780000093
Figure FDA0003373557780000094
4. the multi-joint linkage control method of the mechanical arm according to claim 3, wherein planning the trajectory function of each arm support joint of the mechanical arm at a planning level comprises,
acquiring a motion direction and a speed signal of the tail end of the mechanical arm from a current point to a target point;
calculating the current position and attitude information of the tail end of the mechanical arm based on a base coordinate system fixed by the mechanical arm, a coordinate system of a plurality of arm support joints, a mathematical model of the plurality of arm support joints, joint signals and control instructions;
calculating a track function from the current point to the target point of the tail end of the mechanical arm based on the control instruction;
and calculating a track function corresponding to each arm support joint in the motion process of the mechanical arm based on a kinematics inverse solution algorithm.
5. The multi-joint linkage control method for a robot arm according to claim 4, further comprising,
receiving a control instruction sent by a control device;
judging the configuration of the mechanical arm, determining a reverse kinematics solution algorithm at a control layer, wherein,
when the mechanical arm is a redundant mechanical arm, selecting a redundant degree of freedom numerical solution at the control layer;
and when the mechanical arm is a fixed-rod-length constraint mechanical arm, selecting a spatial six-degree-of-freedom analytic solution at the control layer.
CN202010737493.2A 2020-07-28 2020-07-28 Mechanical arm multi-joint linkage control method Active CN111993414B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010737493.2A CN111993414B (en) 2020-07-28 2020-07-28 Mechanical arm multi-joint linkage control method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010737493.2A CN111993414B (en) 2020-07-28 2020-07-28 Mechanical arm multi-joint linkage control method

Publications (2)

Publication Number Publication Date
CN111993414A CN111993414A (en) 2020-11-27
CN111993414B true CN111993414B (en) 2022-04-12

Family

ID=73468245

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010737493.2A Active CN111993414B (en) 2020-07-28 2020-07-28 Mechanical arm multi-joint linkage control method

Country Status (1)

Country Link
CN (1) CN111993414B (en)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112784428B (en) * 2021-01-29 2023-03-17 华中科技大学鄂州工业技术研究院 DH parameter-based Lagrange dynamics modeling method for concrete pump truck
CN113580128B (en) * 2021-07-09 2022-08-16 国网江西省电力有限公司电力科学研究院 Four-degree-of-freedom mechanical arm control method and transformer substation fire-fighting mechanical arm control method
CN114756063A (en) * 2022-03-30 2022-07-15 徐州徐工施维英机械有限公司 Control method for arm support track planning and pump truck
CN115781690A (en) * 2023-01-05 2023-03-14 北京柏惠维康科技股份有限公司 Control method and device for multi-joint mechanical arm, electronic equipment and storage medium

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP0699621A1 (en) * 1994-08-30 1996-03-06 Paul Lingen Manipulator with several parts for a mobile work apparatus
JP3765212B2 (en) * 1999-09-27 2006-04-12 株式会社日立製作所 Remote working device
CN103176409A (en) * 2013-04-15 2013-06-26 徐州燕大传动与控制技术有限公司 Method for fast and accurately realizing concrete pump truck cantilever crane movement locus
CN103604408A (en) * 2013-11-22 2014-02-26 长沙中联消防机械有限公司 Method, device and system for detecting working state parameters of arm support and engineering machinery
CN104776822A (en) * 2015-04-15 2015-07-15 武汉地和智能有限公司 Multi-section boom posture detecting system and method

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP0699621A1 (en) * 1994-08-30 1996-03-06 Paul Lingen Manipulator with several parts for a mobile work apparatus
JP3765212B2 (en) * 1999-09-27 2006-04-12 株式会社日立製作所 Remote working device
CN103176409A (en) * 2013-04-15 2013-06-26 徐州燕大传动与控制技术有限公司 Method for fast and accurately realizing concrete pump truck cantilever crane movement locus
CN103604408A (en) * 2013-11-22 2014-02-26 长沙中联消防机械有限公司 Method, device and system for detecting working state parameters of arm support and engineering machinery
CN104776822A (en) * 2015-04-15 2015-07-15 武汉地和智能有限公司 Multi-section boom posture detecting system and method

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
高空作业车臂架***快速设计及其运动规划研究;滕儒民;《中国博士学位论文全文数据库 工程科技II辑》;20131015(第10期);第14-96页 *

Also Published As

Publication number Publication date
CN111993414A (en) 2020-11-27

Similar Documents

Publication Publication Date Title
CN111993414B (en) Mechanical arm multi-joint linkage control method
US6919701B2 (en) Robot controller
CN108839025B (en) Motion planning method and device for mobile mechanical arm
DE602005005306T2 (en) Control of the path of a gripper
US8600554B2 (en) System and method for robot trajectory generation with continuous accelerations
WO2015109976A1 (en) Concrete pump truck, and boom control method and control device
CN108340373B (en) Arm support control device, system and method and engineering machinery
CN105382843B (en) Coordination control method for mechanical arm and operation platform in final stage of grabbing
Lippiello et al. Eye-in-hand/eye-to-hand multi-camera visual servoing
EP2218556B1 (en) Controller and control method for a manipulator
CN109358507A (en) A kind of visual servo adaptive tracking control method of time-varying performance boundary constraint
CN115674208B (en) Robot vibration suppression device, control method and robot
US10507585B2 (en) Robot system that displays speed
CN114378827B (en) Dynamic target tracking and grabbing method based on overall control of mobile mechanical arm
US20210347288A1 (en) Crane, a vehicle, and a method of the crane
CN116330267A (en) Control method based on industrial robot wrist singular point calculation
JPH0248400B2 (en)
CN110026986B (en) Multi-arm cooperative operation compliance control method
CN111872927A (en) Redundant mechanical arm and system for controlling redundant mechanical arm
Tongloy et al. An image-based visual servo control system based on an eye-in-hand monocular camera for autonomous robotic grasping
CN113916499B (en) System and method for detecting tracking performance of movable platform optical measuring equipment
DE102022110140A1 (en) COMPLIANT PRESENTATION OF PAYLOAD BY A ROBOT SYSTEM WITH COORDINATED SERIAL AND PARALLEL ROBOTS
CN115354658A (en) Pile body pose regulating and controlling system and regulating and controlling method for pile driver
Bae et al. A dynamic visual servoing of robot manipulator with eye-in-hand camera
CN115416030B (en) Motion planning method for remotely tracking rolling satellite docking ring by mechanical arm

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
CB03 Change of inventor or designer information
CB03 Change of inventor or designer information

Inventor after: Zhu Zhibin

Inventor after: Tang Qiang

Inventor after: Xi Ziyang

Inventor after: Zheng Xu

Inventor after: Li Tengfei

Inventor after: Bu Haiming

Inventor after: Feng Feng

Inventor before: Zhu Zhibin

Inventor before: Xi Ziyang

Inventor before: Zheng Xu

Inventor before: Li Tengfei

Inventor before: Bu Haiming

Inventor before: Feng Feng

Inventor before: Tang Qiang

GR01 Patent grant
GR01 Patent grant