CN116141341A - Method for realizing pointing action of five-degree-of-freedom mechanical arm meeting Cartesian space constraint - Google Patents

Method for realizing pointing action of five-degree-of-freedom mechanical arm meeting Cartesian space constraint Download PDF

Info

Publication number
CN116141341A
CN116141341A CN202310432715.3A CN202310432715A CN116141341A CN 116141341 A CN116141341 A CN 116141341A CN 202310432715 A CN202310432715 A CN 202310432715A CN 116141341 A CN116141341 A CN 116141341A
Authority
CN
China
Prior art keywords
joint
coordinate system
target point
pointed
sequence
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.)
Granted
Application number
CN202310432715.3A
Other languages
Chinese (zh)
Other versions
CN116141341B (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.)
Zhejiang Lab
Original Assignee
Zhejiang Lab
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 Zhejiang Lab filed Critical Zhejiang Lab
Priority to CN202310432715.3A priority Critical patent/CN116141341B/en
Publication of CN116141341A publication Critical patent/CN116141341A/en
Application granted granted Critical
Publication of CN116141341B publication Critical patent/CN116141341B/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/1602—Programme controls characterised by the control system, structure, architecture
    • 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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)

Abstract

The invention discloses a method for realizing pointing action of a five-degree-of-freedom mechanical arm, which meets the constraint of Cartesian space, and comprises the following steps: acquiring the length of each joint rod of the mechanical arm, a target point to be pointed and a track sequence of the target point to be pointed; taking the collineation of the outward coordinate axis of the wrist coordinate system and the target point to be pointed as a main optimization target, and constructing a nonlinear optimization equation set; taking a target point track sequence to be pointed as input, and carrying out optimization solution on a nonlinear optimization equation set by using a sequence least square method; if the variation of the joint variable obtained by the optimization solution and the joint variable of the previous frame exceeds the allowable range, selecting the joint variable of the previous frame to be assigned to the joint variable sequence; otherwise, adding the joint variable obtained by the optimization solution into a joint variable sequence, and performing angle interpolation on the joint variable sequence; and transmitting the joint angle sequence data subjected to angle interpolation to a robot controller so as to drive the 5R mechanical arm to point to the target point track by using a servo control method.

Description

Method for realizing pointing action of five-degree-of-freedom mechanical arm meeting Cartesian space constraint
Technical Field
The invention belongs to the technical field of motion generation of mechanical arms of navigation robots, and particularly relates to a method for realizing pointing motion of a five-degree-of-freedom mechanical arm, which meets Cartesian space constraint.
Background
The action direction of the robot is widely applied to service industry, for example, in scenes such as scenic spots, museums and the like, the robot can be used as a tour guide to provide multiple functions such as scenic spot explanation, tour guide, question-answer photographing and the like for customers; in the scenes of government affairs hall, enterprise foreground, transportation hub, etc., the robot can be used as a customer service personnel for business consultation and handling, and provides services such as customer service, welcome reception, window guidance, product consultation, business handling, etc. The pointing action is a guiding action, can generate actions by the track of the target point, can be inserted between offline actions to perform online pointing, improves the universality and stability of the offline actions, and can provide a preparatory action for grabbing the mechanical arm in advance.
In the process of implementing the present invention, the inventor finds that at least the following problems exist in the prior art:
currently, in practical application of motion pointing of a robot, a series of offline motions are generally predefined to meet the use requirement of the robot, and when the guiding environment changes, the offline motions often need to be reconfigured or taught again. Meanwhile, the configuration of the offline motion needs to meet the Cartesian space constraints of the environment, the rod piece and the robot body, and the offline motion joint track needs to be generated by manual debugging. Meanwhile, the pointing action of the robot often needs to point to a far point beyond the operation space of the mechanical arm, and is not suitable for using the inverse kinematics method.
Disclosure of Invention
The embodiment of the application aims to provide a method for realizing the pointing action of a five-degree-of-freedom mechanical arm meeting the constraint of Cartesian space, so as to solve the defect that offline action in a robot needs manual debugging after a guiding environment changes.
According to a first aspect of an embodiment of the present application, a method for implementing a pointing action of a five-degree-of-freedom mechanical arm that satisfies a cartesian space constraint is provided, including:
acquiring the length of each joint rod of the mechanical arm, a target point to be pointed and a track sequence of the target point to be pointed;
taking the world coordinate system, the relation among the current 5R mechanical arm joint coordinate systems and the length of each joint rod as parameters, taking the target point to be pointed as a variable, taking the collineation of the outward coordinate axis of the wrist coordinate system and the target point to be pointed as a main optimization target, and converting the Cartesian space constraint on the robot body, the Cartesian space constraint on the environment and the angle constraint of each joint into the limiting conditions of a nonlinear optimization equation set to construct the nonlinear optimization equation set;
taking the target point track sequence to be pointed as input, and carrying out optimization solution on the nonlinear optimization equation set by using a sequence least square method;
if the variation of the joint variable obtained by the optimization solution and the joint variable of the previous frame exceeds the allowable range, selecting the joint variable of the previous frame to be assigned to the joint variable sequence; if the variation of the joint variable obtained by the optimization solution and the joint variable of the previous frame is in the allowable range, adding the joint variable obtained by the optimization solution into a joint variable sequence, and performing angle interpolation on the joint variable sequence;
and transmitting the joint angle sequence data subjected to angle interpolation to a robot controller so as to drive the 5R mechanical arm to point to the target point track by using a servo control method.
Further, a forward kinematics model of the 5R mechanical arm is built according to a world coordinate system of the 5R mechanical arm, each joint coordinate system and the length of a rod between joints, and the relationship between the world coordinate system and the joint coordinate system of the current 5R mechanical arm is obtained according to the forward kinematics model.
Further, the main optimization objective is specifically: minimizing the offset of the vector formed by the pointed target point and the origin of the wrist coordinate system and the vector of the elbow rod in the world coordinate system, the projection of the pointed target point in the vector direction of the elbow rod, and the inner product of the vector formed by the pointed target point and the origin of the wrist coordinate system and the x and y axes of the wrist coordinate system.
Further, before constructing a nonlinear optimization equation set, an L2 norm of a deviation between the joint variable and a previous solution result is introduced as a sub-optimization target.
Further, the constructed nonlinear optimization equation set is:
Figure SMS_1
，
wherein the method comprises the steps of
Figure SMS_5
Three joint angles of the shoulder, the elbow and the wrist, respectively,
Figure SMS_13
Is->
Figure SMS_20
Corresponding weight coefficient, ++>
Figure SMS_4
For the offset of the target point to be pointed to in the Z-axis direction relative to the origin of the world coordinate system, +.>
Figure SMS_14
、
Figure SMS_22
And the same is done;
Figure SMS_29
Transformation matrix representing wrist coordinate system relative to world coordinate system, < >>
Figure SMS_7
Representation of the wrist X-axis in world coordinate system,/->
Figure SMS_10
Representing the wrist Y-axisRepresentation under world coordinate system, +.>
Figure SMS_18
Representation of the wrist Z-axis in world coordinate system,/->
Figure SMS_26
Representing the origin offset of the wrist coordinate system relative to the world coordinate system,/->
Figure SMS_6
、
Figure SMS_15
、
Figure SMS_24
For the position of the origin of the wrist coordinate system relative to the origin of the world coordinate system,/->
Figure SMS_31
Representing the origin offset of the target point to be pointed to with respect to the relative world coordinate system,/->
Figure SMS_8
、
Figure SMS_16
、
Figure SMS_23
、
Figure SMS_30
、
Figure SMS_2
、
Figure SMS_12
、
Figure SMS_21
And the same is done;
Figure SMS_28
the ith optimization result for the jth joint,/->
Figure SMS_3
For the j-th joint, for the i-1 st optimization result to be directed to the target point,/th joint>
Figure SMS_11
For Cartesian space constraints for the environment, +.>
Figure SMS_19
For Cartesian space constraints for the robot body, +.>
Figure SMS_27
、
Figure SMS_9
Respectively->
Figure SMS_17
And->
Figure SMS_25
Corresponding weight coefficients.
Further, the joint variable sequence is subjected to angle interpolation by using a modified Akima segmentation three-time Hermite interpolation method.
According to a second aspect of embodiments of the present application, there is provided a five-degree-of-freedom mechanical arm pointing motion implementation apparatus satisfying a cartesian space constraint, including:
the acquisition module is used for acquiring the length of each joint rod of the mechanical arm, the target point to be pointed and the track sequence of the target point to be pointed;
the construction module is used for converting Cartesian space constraint aiming at a robot body, cartesian space constraint aiming at an environment and angle constraint of each joint into a constraint condition of a nonlinear optimization equation set by taking a world coordinate system, a relation among current 5R mechanical arm joint coordinate systems and length of each joint rod as parameters, taking the target point to be pointed as a variable, taking an outward coordinate axis of a wrist coordinate system and the target point to be pointed as main optimization targets, and constructing the nonlinear optimization equation set;
the solving module is used for taking the target point track sequence to be pointed as input and carrying out optimization solving on the nonlinear optimization equation set by using a sequence least square method;
the angle interpolation module is used for selecting the joint variable of the previous frame to be assigned to the joint variable sequence if the variation of the joint variable obtained by the optimization solution and the joint variable of the previous frame exceeds the allowable range; if the variation of the joint variable obtained by the optimization solution and the joint variable of the previous frame is in the allowable range, adding the joint variable obtained by the optimization solution into a joint variable sequence, and performing angle interpolation on the joint variable sequence;
and the transmission module is used for transmitting the joint angle sequence data subjected to angle interpolation to a robot controller so as to drive the 5R mechanical arm to point to the target point track by using a servo control method.
According to a third aspect of embodiments of the present application, there is provided an electronic device, including:
one or more processors;
a memory for storing one or more programs;
the one or more programs, when executed by the one or more processors, cause the one or more processors to implement the method of the first aspect.
According to a fourth aspect of embodiments of the present application, there is provided a computer readable storage medium having stored thereon computer instructions which, when executed by a processor, implement the steps of the method according to the first aspect.
The technical scheme provided by the embodiment of the application can comprise the following beneficial effects:
according to the embodiment, the established pointing motion nonlinear equation is solved by adopting a sequence least square method, the problems that inverse kinematics cannot obtain an inverse solution for a gesture-free information point and space constraint exists in the environment are solved, and the fact that a robot can directly point to a given gesture-free pointing point through a solving result of the nonlinear equation is achieved. The online pointing action of the 5R mechanical arm on the track of the target point without gesture information is realized, the online pointing problem of the mechanical arm without the target point with gesture information is solved, the kinematic singular point of the mechanical arm can be avoided, and the three-dimensional space constraint is satisfied.
It is to be understood that both the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the application.
Drawings
The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the application and together with the description, serve to explain the principles of the application.
FIG. 1 is a flow chart illustrating a method for implementing a five degree of freedom robotic pointing action that satisfies Cartesian space constraints, according to an exemplary embodiment.
Fig. 2 is a schematic diagram of a navigation robot shown according to an exemplary embodiment.
Fig. 3 is a schematic diagram of a robot configuration shown according to an exemplary embodiment.
Fig. 4 is a block diagram illustrating a five-degree-of-freedom robotic pointing motion implementation satisfying cartesian space constraints, according to an example embodiment.
Fig. 5 is a schematic diagram of an electronic device, according to an example embodiment.
Detailed Description
Reference will now be made in detail to exemplary embodiments, examples of which are illustrated in the accompanying drawings. When the following description refers to the accompanying drawings, the same numbers in different drawings refer to the same or similar elements, unless otherwise indicated. The implementations described in the following exemplary examples are not representative of all implementations consistent with the present application.
The terminology used in the present application is for the purpose of describing particular embodiments only and is not intended to be limiting of the present application. As used in this application and the appended claims, the singular forms "a," "an," and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise. It should also be understood that the term "and/or" as used herein refers to and encompasses any or all possible combinations of one or more of the associated listed items.
It should be understood that although the terms first, second, third, etc. may be used herein to describe various information, these information should not be limited by these terms. These terms are only used to distinguish one type of information from another. For example, a first message may also be referred to as a second message, and similarly, a second message may also be referred to as a first message, without departing from the scope of the present application. The word "if" as used herein may be interpreted as "at … …" or "at … …" or "responsive to a determination", depending on the context.
Fig. 1 is a flowchart illustrating a method for implementing a pointing action of a five-degree-of-freedom mechanical arm that satisfies a cartesian space constraint according to an exemplary embodiment, and the method is implemented through a terminal, and is applied to a navigation robot as shown in fig. 2, and a robot configuration is shown in fig. 3, and the method may include the following steps:
step S10: acquiring the length of each joint rod of the mechanical arm, a target point to be pointed and a track sequence of the target point to be pointed;
step S11: taking the world coordinate system, the relation among the current 5R mechanical arm joint coordinate systems and the length of each joint rod as parameters, taking the target point to be pointed as a variable, taking the collineation of the outward coordinate axis of the wrist coordinate system and the target point to be pointed as a main optimization target, and converting the Cartesian space constraint on the robot body, the Cartesian space constraint on the environment and the angle constraint of each joint into the limiting conditions of a nonlinear optimization equation set to construct the nonlinear optimization equation set;
step S12: taking the target point track sequence to be pointed as input, and carrying out optimization solution on the nonlinear optimization equation set by using a sequence least square method;
step S13: if the variation of the joint variable obtained by the optimization solution and the joint variable of the previous frame exceeds the allowable range, selecting the joint variable of the previous frame to be assigned to the joint variable sequence; if the variation of the joint variable obtained by the optimization solution and the joint variable of the previous frame is in the allowable range, adding the joint variable obtained by the optimization solution into a joint variable sequence, and performing angle interpolation on the joint variable sequence;
step S14: and transmitting the joint angle sequence data subjected to angle interpolation to a robot controller so as to drive the 5R mechanical arm to point to the target point track by using a servo control method.
According to the embodiment, the established pointing motion nonlinear equation is solved by adopting a sequence least square method, the problems that inverse kinematics cannot obtain an inverse solution for a gesture-free information point and space constraint exists in the environment are solved, and the fact that a robot can directly point to a given gesture-free pointing point through a solving result of the nonlinear equation is achieved. The online pointing action of the 5R mechanical arm on the track of the target point without gesture information is realized, the online pointing problem of the mechanical arm without the target point with gesture information is solved, the kinematic singular point of the mechanical arm can be avoided, and the three-dimensional space constraint is satisfied.
In the implementation of step S10, the length of each joint rod of the mechanical arm, the target point to be pointed and the track sequence of the target point to be pointed are obtained;
specifically, the target point to be pointed in the real-time running process needs to be determined by a task performed by a robot, a three-dimensional target point provided by a visual map is generally used as the target point to be pointed, a track sequence of the target point to be pointed is obtained by off-line programming, and the track sequence is represented by a fixed group of three-dimensional target points or dynamic curves of the three-dimensional target points.
In the implementation of step S11, the relationship among the world coordinate system, the current 5R mechanical arm joint coordinate system and the length of each joint rod are taken as parameters, the target point to be pointed is taken as a variable, the out coordinate axis of the wrist coordinate system and the target point to be pointed are collinear, and the cartesian space constraint for the robot body, the cartesian space constraint for the environment and the angle constraint of each joint are converted into the constraint conditions of a nonlinear optimization equation set, so as to construct the nonlinear optimization equation set;
in particular, this step may comprise the sub-steps of:
step S21: establishing a forward kinematics model of the 5R mechanical arm according to the world coordinate system of the 5R mechanical arm, each joint coordinate system and the inter-joint rod length to obtain the analysis type of five joint variables (namely, the homogeneous transformation of the wrist coordinate system of the 5R mechanical arm and the world coordinate system) of the position of the tail end of the mechanical arm, the origin of each joint coordinate system under the world coordinate system;
Figure SMS_32
the homogeneous transformation (denoted as T) of each joint corresponding to the last joint coordinate system is generally obtained by DH modeling and rotation,
Figure SMS_33
the wrist joint is transformed uniformly corresponding to the world coordinate system.
Here will be
Figure SMS_34
Summarizing together, representing homogeneous transformation of the shoulder with respect to the robot coordinate system as passing +.>
Figure SMS_35
Homogeneous transformation results for three joint variables.
Step S22: giving the position of the target point to be pointed under the world coordinate system, obtaining the three-dimensional coordinate of the target point to be pointed at the world coordinate system through the forward kinematics model, converting the Cartesian space constraint for the robot body, the Cartesian space constraint for the environment and the angle constraint of each joint into the limiting condition of a nonlinear optimization equation set, and constructing the nonlinear optimization equation set, wherein the established optimization target comprises: and enabling the target point to be pointed to be positioned in the positive direction of the z axis of the wrist coordinate system, and minimizing the offset of the target point to be pointed to be positioned on the x and y axes of the wrist coordinate system, the inner product of a vector formed by the pointed target point and the origin of the wrist coordinate system and the x and y axes of the wrist coordinate system, and the deviation between the last optimization solving result.
In particular, the cartesian space constraints for the robot body can be expressed as:
Figure SMS_36
in the method, in the process of the invention,
Figure SMS_37
indicating the position of the robot shoulder joint corresponding to the world coordinate system,/->
Figure SMS_38
Representing the position of the robot elbow joint corresponding to the world coordinate system,/->
Figure SMS_39
Indicating the position of the robot shoulder joint corresponding to the world coordinate system,/->
Figure SMS_40
May be expressed as a linear constraint or a nonlinear constraint condition for the robot body structure, such as a robot body collision constraint.
The cartesian space constraints for an environment can be expressed as:
Figure SMS_41
in the method, in the process of the invention,
Figure SMS_42
may be expressed as a linear constraint or a non-linear constraint for the environment, e.g. obstacles present in the scene, coordinate constraints in the scene.
Joint constraints can be expressed as:
Figure SMS_43
，1≤j≤5
in the method, in the process of the invention,
Figure SMS_44
minimum value of j-th joint variable of 5R mechanical arm, < >>
Figure SMS_45
For the actual value of the jth joint variable of the 5R mechanical arm,>
Figure SMS_46
the maximum value of the j-th joint variable of the 5R mechanical arm.
In the implementation of step S12, the sequence of the target point track to be pointed is used as input, and the nonlinear optimization equation set is optimized and solved by using a sequence least square method;
specifically, the sequence of the target point track to be pointed is taken as input, in step S11, the outer coordinate axis of the wrist coordinate system and the target point to be pointed are taken as main optimization targets, in addition, in order to improve the smoothness of the pointing action track, the L2 norm of the deviation between the joint variable and the previous solving result is introduced as a secondary optimization target, so that the constructed nonlinear optimization equation set is as follows:
Figure SMS_47
，
wherein the method comprises the steps of
Figure SMS_50
Three joint angles of the shoulder, the elbow and the wrist, respectively,
Figure SMS_56
Is->
Figure SMS_64
Corresponding weight coefficient, ++>
Figure SMS_49
For the offset of the target point to be pointed to in the Z-axis direction relative to the origin of the world coordinate system, +.>
Figure SMS_57
、
Figure SMS_65
And the same is done;
Figure SMS_72
Transformation matrix representing wrist coordinate system relative to world coordinate system, < >>
Figure SMS_51
Representation of the wrist X-axis in world coordinate system,/->
Figure SMS_60
Representation of the wrist Y-axis in world coordinate system,/->
Figure SMS_68
Representation of the wrist Z-axis in world coordinate system,/->
Figure SMS_75
Representing the origin offset of the wrist coordinate system relative to the world coordinate system,/->
Figure SMS_53
、
Figure SMS_59
、
Figure SMS_67
For the position of the origin of the wrist coordinate system relative to the origin of the world coordinate system,/->
Figure SMS_74
Representing the origin offset of the target point to be pointed to with respect to the relative world coordinate system,/->
Figure SMS_55
、
Figure SMS_62
、
Figure SMS_70
、
Figure SMS_76
、
Figure SMS_48
、
Figure SMS_58
、
Figure SMS_66
And the same is done;
Figure SMS_73
the ith optimization result for the jth joint,/->
Figure SMS_54
For the j-th joint, for the i-1 st optimization result to be directed to the target point,/th joint>
Figure SMS_61
For Cartesian space constraints for the environment, +.>
Figure SMS_71
For Cartesian space constraints for the robot body, +.>
Figure SMS_77
、
Figure SMS_52
Respectively->
Figure SMS_63
And->
Figure SMS_69
Corresponding weight coefficients.
Specifically, f 1 When the mechanical arm points to the target point to be pointed, the target point should be located outside the direction of the wrist rotating shaft of the mechanical arm (the Z axis of the mechanical arm is defined as the positive direction of the rotating shaft of the mechanical arm, namely the Z axis is outwards, the Y axis is uniformly defined as the left side of the robot, the X axis is obtained by taking the right hand rule of the determined X, Y axis), f 2 F 3 Limiting the target point to be pointed to in the direction of the rotating shaft, f 4 、f 5 The vector representing the coordinate system of the wrist to the target point to be pointed at should be perpendicular to the axis of the wrist coordinate X, Y (condition 1,2,3 is emphasized), f 6 Representing that two or more points to be pointed to appear, minimizing both of themThe deviation between the pitch variables, T, represents a homogeneous transformation matrix of the wrist coordinate system relative to the world coordinate system,
Figure SMS_80
representation of the wrist X-axis in world coordinate system,/->
Figure SMS_83
Representation of the wrist Y-axis in world coordinate system,/->
Figure SMS_85
Representation of the wrist Z-axis in world coordinate system,/->
Figure SMS_79
Representing the origin offset of the wrist coordinate system relative to the world coordinate system,/->
Figure SMS_81
、
Figure SMS_84
、
Figure SMS_86
Respectively, the position of the wrist coordinate system origin with respect to the world coordinate system origin. Where write represents the wrist, world represents the world coordinate system, superscript is the reference coordinate system, and subscript is the actual coordinate system.
Figure SMS_78
The ith optimization result for the jth joint,/->
Figure SMS_82
The result is optimized for the j-th joint for the i-1 th time of aiming at the target point. It should be noted that if a series of optimization solutions are performed in non-real time for the target point track sequence to be pointed to, the ith optimization result for the ith point in the sequence is represented by the ith time, the i-1 optimization result for the ith-1 point in the sequence is represented by the ith time, the deviation between the two adjacent optimization solutions is minimized in the optimization process, for example, the joint angle output for the first pointing point is calculated by optimizingIn the process of solving the joint angle of the second pointing point, minimizing the deviation between the first solving joint angle; if the joint angle is optimized and solved for the target point to be pointed in real time, the ith time represents the current optimizing result, and the (i-1) th time represents the last optimizing result of me.
And solving the optimization solving equation by a sequence least square method to obtain three joint angles of the shoulder, one joint angle of the elbow and one joint angle of the wrist.
In the implementation of the step S13, if the variation of the joint variable obtained by the optimization solution and the joint variable of the previous frame exceeds the allowable range, the joint variable of the previous frame is selected to be assigned to the joint variable sequence; if the variation of the joint variable obtained by the optimization solution and the joint variable of the previous frame is in the allowable range, adding the joint variable obtained by the optimization solution into a joint variable sequence, and performing angle interpolation on the joint variable sequence;
specifically, the method can perform real-time interpolation between two frames, or perform interpolation of the whole sequence in non-real time after obtaining a joint variable sequence.
For interpolation of the whole sequence, after the optimized joint variable sequence is obtained, because the sequence has certain discreteness, in order to ensure the smoothness of the action of the mechanical arm, a denser joint track is obtained by using a modified Akima segmentation three-time Hermite interpolation method, and the obtained joint variable sequence can avoid overshoot and achieve better effect than quintic spline interpolation.
More specifically, the action key frame is corrected, the Akima is segmented for three Hermite interpolation, and the angle corresponding to the current position and the target position satisfies the following relation:
Figure SMS_87
in the method, in the process of the invention,
Figure SMS_88
representing the current angle of connection +.>
Figure SMS_89
(corresponding time->
Figure SMS_90
) And the next angle->
Figure SMS_91
(corresponding time->
Figure SMS_92
) About time->
Figure SMS_93
When two platform areas with different slopes meet, the modification made to the original Akima algorithm gives more weight to the side with the slope closer to zero, and the modification prioritizes the side closer to horizontal, so that overshoot is more intuitive and avoided. In particular, the algorithm connects three or more consecutive collinear points with a straight line whenever there are such points, thereby avoiding overshooting. The value of the derivative di at the sampling point thetai is weighted by the two correction weights given.
Further, obtain
Figure SMS_94
The formula is a general formula for improving the akima algorithm, the interpolation result is expressed as a cubic polynomial, and the weight c of the formula is calculated from the derivative result obtained before.
According to the polynomial coefficient of the cubic polynomial, the 5R mechanical arm can generate a joint angle sequence, a joint angular velocity sequence and a joint angular acceleration sequence of the mechanical arm according to a certain time interval, so that the conversion of the mechanical arm between key frames generated by improving an akima interpolation method is realized.
In the implementation of step S14, the joint angle sequence data after the angle interpolation is transmitted to the robot controller, so as to drive the 5R manipulator to point to the target point track by using the servo control method.
It should be noted that the implementation of step S14 is a conventional technical means in the art, and will not be described herein.
Corresponding to the embodiment of the method for realizing the pointing action of the five-degree-of-freedom mechanical arm meeting the Cartesian space constraint, the application also provides an embodiment of a device for realizing the pointing action of the five-degree-of-freedom mechanical arm meeting the Cartesian space constraint.
FIG. 4 is a block diagram illustrating a five-degree-of-freedom robotic pointing motion implementation satisfying Cartesian space constraints according to an exemplary embodiment. Referring to fig. 4, the apparatus may include:
the acquisition module 21 is used for acquiring the length of each joint rod of the mechanical arm, the target point to be pointed and the track sequence of the target point to be pointed;
the construction module 22 is configured to convert cartesian space constraint for the robot body, cartesian space constraint for the environment, and angle constraint for each joint into constraint conditions of a nonlinear optimization equation set, and construct the nonlinear optimization equation set by using the world coordinate system, the current relationship among the 5R mechanical arm joint coordinate systems, and the length of each joint rod as parameters, using the target point to be pointed as a variable, using the axis of the wrist coordinate system directed outwards and the target point to be pointed as main optimization targets;
the solving module 23 is configured to perform optimization solving on the nonlinear optimization equation set by using a sequence least square method with the target point track sequence to be pointed as an input;
the angle interpolation module 24 is configured to select the joint variable of the previous frame to be assigned to the joint variable sequence if the variation of the joint variable obtained by the optimization solution and the joint variable of the previous frame exceeds the allowable range; if the variation of the joint variable obtained by the optimization solution and the joint variable of the previous frame is in the allowable range, adding the joint variable obtained by the optimization solution into a joint variable sequence, and performing angle interpolation on the joint variable sequence;
and the transmission module 25 is used for transmitting the joint angle sequence data subjected to angle interpolation to a robot controller so as to drive the 5R mechanical arm to point to the target point track by using a servo control method.
The specific manner in which the various modules perform the operations in the apparatus of the above embodiments have been described in detail in connection with the embodiments of the method, and will not be described in detail herein.
For the device embodiments, reference is made to the description of the method embodiments for the relevant points, since they essentially correspond to the method embodiments. The apparatus embodiments described above are merely illustrative, wherein the elements illustrated as separate elements may or may not be physically separate, and the elements shown as elements may or may not be physical elements, may be located in one place, or may be distributed over a plurality of network elements. Some or all of the modules may be selected according to actual needs to achieve the purposes of the present application. Those of ordinary skill in the art will understand and implement the present invention without undue burden.
Correspondingly, the application also provides electronic equipment, which comprises: one or more processors; a memory for storing one or more programs; when the one or more programs are executed by the one or more processors, the one or more processors implement the five-degree-of-freedom robotic arm pointing action implementation method that satisfies the cartesian space constraints as described above. As shown in fig. 5, a hardware structure diagram of an arbitrary device with data processing capability, where the five-degree-of-freedom mechanical arm pointing action implementation method satisfying the cartesian space constraint is provided in the embodiment of the present invention, except for the processor, the memory and the network interface shown in fig. 5, the arbitrary device with data processing capability in the embodiment is generally according to the actual function of the arbitrary device with data processing capability, and may further include other hardware, which is not described herein.
Correspondingly, the application also provides a computer readable storage medium, on which computer instructions are stored, which when executed by a processor, implement the five-degree-of-freedom mechanical arm pointing action implementation method meeting the cartesian space constraint. The computer readable storage medium may be an internal storage unit, such as a hard disk or a memory, of any of the data processing enabled devices described in any of the previous embodiments. The computer readable storage medium may also be an external storage device, such as a plug-in hard disk, a Smart Media Card (SMC), an SD Card, a Flash memory Card (Flash Card), or the like, provided on the device. Further, the computer readable storage medium may include both internal storage units and external storage devices of any device having data processing capabilities. The computer readable storage medium is used for storing the computer program and other programs and data required by the arbitrary data processing apparatus, and may also be used for temporarily storing data that has been output or is to be output.
Other embodiments of the present application will be apparent to those skilled in the art from consideration of the specification and practice of the disclosure herein. This application is intended to cover any variations, uses, or adaptations of the application following, in general, the principles of the application and including such departures from the present disclosure as come within known or customary practice within the art to which the application pertains.
It is to be understood that the present application is not limited to the precise arrangements and instrumentalities shown in the drawings, which have been described above, and that various modifications and changes may be effected without departing from the scope thereof.

Claims (9)

1. A method for realizing pointing action of a five-degree-of-freedom mechanical arm meeting Cartesian space constraint is characterized by comprising the following steps:
acquiring the length of each joint rod of the mechanical arm, a target point to be pointed and a track sequence of the target point to be pointed;
taking the world coordinate system, the relation among the current 5R mechanical arm joint coordinate systems and the length of each joint rod as parameters, taking the target point to be pointed as a variable, taking the collineation of the outward coordinate axis of the wrist coordinate system and the target point to be pointed as a main optimization target, and converting the Cartesian space constraint on the robot body, the Cartesian space constraint on the environment and the angle constraint of each joint into the limiting conditions of a nonlinear optimization equation set to construct the nonlinear optimization equation set;
taking the target point track sequence to be pointed as input, and carrying out optimization solution on the nonlinear optimization equation set by using a sequence least square method;
if the variation of the joint variable obtained by the optimization solution and the joint variable of the previous frame exceeds the allowable range, selecting the joint variable of the previous frame to be assigned to the joint variable sequence; if the variation of the joint variable obtained by the optimization solution and the joint variable of the previous frame is in the allowable range, adding the joint variable obtained by the optimization solution into a joint variable sequence, and performing angle interpolation on the joint variable sequence;
and transmitting the joint angle sequence data subjected to angle interpolation to a robot controller so as to drive the 5R mechanical arm to point to the target point track by using a servo control method.
2. The method according to claim 1, wherein a forward kinematics model of the 5R robot is built according to a world coordinate system, each joint coordinate system and an inter-joint rod length of the 5R robot, and the relationship between the world coordinate system and the current joint coordinate system of the 5R robot is obtained according to the forward kinematics model.
3. The method according to claim 1, characterized in that the main optimization objective is in particular: and enabling the target point to be pointed to be positioned in the positive direction of the z axis of the wrist coordinate system, and minimizing the offset of the target point to be pointed to be positioned on the x and y axes of the wrist coordinate system, the inner product of a vector formed by the pointed target point and the origin of the wrist coordinate system and the x and y axes of the wrist coordinate system, and the deviation between the last optimization solving result.
4. The method according to claim 1, characterized in that, before constructing the system of nonlinear optimization equations, the L2 norm of the deviation between the joint variables and the previous solution is introduced as the suboptimal objective.
5. The method of claim 4, wherein the set of established nonlinear optimization equations is:
Figure QLYQS_1
，
wherein the method comprises the steps of
Figure QLYQS_2
Three joint angles of the shoulder, the elbow and the wrist, respectively,
Figure QLYQS_11
Is->
Figure QLYQS_20
Corresponding weight coefficient, ++>
Figure QLYQS_6
For the offset of the target point to be pointed to in the Z-axis direction relative to the origin of the world coordinate system, +.>
Figure QLYQS_16
、
Figure QLYQS_24
And the same is done;
Figure QLYQS_30
Transformation matrix representing wrist coordinate system relative to world coordinate system, < >>
Figure QLYQS_9
Representation of the wrist X-axis in world coordinate system,/->
Figure QLYQS_17
Representation of the wrist Y-axis in world coordinate system,/->
Figure QLYQS_25
Representation of the wrist Z-axis in world coordinate system,/->
Figure QLYQS_31
Representing the origin offset of the wrist coordinate system relative to the world coordinate system,/->
Figure QLYQS_7
、
Figure QLYQS_12
、
Figure QLYQS_19
For the position of the origin of the wrist coordinate system relative to the origin of the world coordinate system,/->
Figure QLYQS_28
Representing the origin offset of the target point to be pointed to with respect to the relative world coordinate system,/->
Figure QLYQS_5
、
Figure QLYQS_10
、
Figure QLYQS_18
、
Figure QLYQS_26
、
Figure QLYQS_4
、
Figure QLYQS_13
、
Figure QLYQS_21
And the same is done;
Figure QLYQS_27
The ith optimization result for the jth joint,/->
Figure QLYQS_3
For the j-th joint, for the i-1 st optimization result to be directed to the target point,/th joint>
Figure QLYQS_14
For Cartesian space constraints for the environment, +.>
Figure QLYQS_22
For Cartesian space constraints for the robot body, +.>
Figure QLYQS_29
、
Figure QLYQS_8
Respectively->
Figure QLYQS_15
And->
Figure QLYQS_23
Corresponding weight coefficients.
6. The method of claim 1, wherein the sequence of joint variables is interpolated angularly using a modified Akima piecewise three-time Hermite interpolation method.
7. The utility model provides a five degrees of freedom arms directional action realization device that satisfies cartesian space constraint which characterized in that includes:
the acquisition module is used for acquiring the length of each joint rod of the mechanical arm, the target point to be pointed and the track sequence of the target point to be pointed;
the construction module is used for converting Cartesian space constraint aiming at a robot body, cartesian space constraint aiming at an environment and angle constraint of each joint into a constraint condition of a nonlinear optimization equation set by taking a world coordinate system, a relation among current 5R mechanical arm joint coordinate systems and length of each joint rod as parameters, taking the target point to be pointed as a variable, taking an outward coordinate axis of a wrist coordinate system and the target point to be pointed as main optimization targets, and constructing the nonlinear optimization equation set;
the solving module is used for taking the target point track sequence to be pointed as input and carrying out optimization solving on the nonlinear optimization equation set by using a sequence least square method;
the angle interpolation module is used for selecting the joint variable of the previous frame to be assigned to the joint variable sequence if the variation of the joint variable obtained by the optimization solution and the joint variable of the previous frame exceeds the allowable range; if the variation of the joint variable obtained by the optimization solution and the joint variable of the previous frame is in the allowable range, adding the joint variable obtained by the optimization solution into a joint variable sequence, and performing angle interpolation on the joint variable sequence;
and the transmission module is used for transmitting the joint angle sequence data subjected to angle interpolation to a robot controller so as to drive the 5R mechanical arm to point to the target point track by using a servo control method.
8. An electronic device, comprising:
one or more processors;
a memory for storing one or more programs;
the one or more programs, when executed by the one or more processors, cause the one or more processors to implement the method of any of claims 1-6.
9. A computer readable storage medium having stored thereon computer instructions which, when executed by a processor, implement the steps of the method according to any of claims 1-6.
CN202310432715.3A 2023-04-21 2023-04-21 Method for realizing pointing action of five-degree-of-freedom mechanical arm meeting Cartesian space constraint Active CN116141341B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310432715.3A CN116141341B (en) 2023-04-21 2023-04-21 Method for realizing pointing action of five-degree-of-freedom mechanical arm meeting Cartesian space constraint

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310432715.3A CN116141341B (en) 2023-04-21 2023-04-21 Method for realizing pointing action of five-degree-of-freedom mechanical arm meeting Cartesian space constraint

Publications (2)

Publication Number Publication Date
CN116141341A true CN116141341A (en) 2023-05-23
CN116141341B CN116141341B (en) 2023-08-08

Family

ID=86362220

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310432715.3A Active CN116141341B (en) 2023-04-21 2023-04-21 Method for realizing pointing action of five-degree-of-freedom mechanical arm meeting Cartesian space constraint

Country Status (1)

Country Link
CN (1) CN116141341B (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN118305813A (en) * 2024-06-12 2024-07-09 苏州远昌智能科技有限公司 Teleoperation control method and system for humanoid robot

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2005069734A (en) * 2003-08-20 2005-03-17 Tokyo Univ Of Marine Science & Technology Method of instructing spatial position of object by pointing action
US20130211597A1 (en) * 2012-02-14 2013-08-15 Jerry Neil SOMMERVILLE Control System That Guides A Robot Or Articulated Device With A Laser Distance Meter For 3D Motion, Or Guides A Robot Or Articulated Device With A Computer Pointing Device (Such As A Mouse) For 2D Motion
CN105068536A (en) * 2015-08-13 2015-11-18 吉林大学 Moving substrate track planner achieved based on nonlinear optimization method
CN105773641A (en) * 2014-12-24 2016-07-20 肖伟 Finger pointer
CN107398903A (en) * 2017-09-04 2017-11-28 杭州电子科技大学 The method for controlling trajectory of industrial machinery arm actuating station
CN111185923A (en) * 2020-01-14 2020-05-22 深圳众为兴技术股份有限公司 Robot control device and control method
US20210053220A1 (en) * 2019-08-23 2021-02-25 Realtime Robotics, Inc. Motion planning for robots to optimize velocity while maintaining limits on acceleration and jerk
WO2021065196A1 (en) * 2019-10-03 2021-04-08 Mitsubishi Electric Corporation Method and system for trajectory optimization for nonlinear robotic systems with geometric constraints
CN112805127A (en) * 2018-10-06 2021-05-14 百超激光有限公司 Method and apparatus for creating robot control program
US20210308862A1 (en) * 2020-04-03 2021-10-07 Fanuc Corporation Fast robot motion optimization with distance field
CN115778445A (en) * 2022-12-07 2023-03-14 佗道医疗科技有限公司 Laser guidance verification method for tail end of mechanical arm
CN115922728A (en) * 2023-01-03 2023-04-07 之江实验室 Robot pointing motion control method, device, electronic device and storage medium

Patent Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2005069734A (en) * 2003-08-20 2005-03-17 Tokyo Univ Of Marine Science & Technology Method of instructing spatial position of object by pointing action
US20130211597A1 (en) * 2012-02-14 2013-08-15 Jerry Neil SOMMERVILLE Control System That Guides A Robot Or Articulated Device With A Laser Distance Meter For 3D Motion, Or Guides A Robot Or Articulated Device With A Computer Pointing Device (Such As A Mouse) For 2D Motion
CN105773641A (en) * 2014-12-24 2016-07-20 肖伟 Finger pointer
CN105068536A (en) * 2015-08-13 2015-11-18 吉林大学 Moving substrate track planner achieved based on nonlinear optimization method
CN107398903A (en) * 2017-09-04 2017-11-28 杭州电子科技大学 The method for controlling trajectory of industrial machinery arm actuating station
CN112805127A (en) * 2018-10-06 2021-05-14 百超激光有限公司 Method and apparatus for creating robot control program
US20210053220A1 (en) * 2019-08-23 2021-02-25 Realtime Robotics, Inc. Motion planning for robots to optimize velocity while maintaining limits on acceleration and jerk
WO2021065196A1 (en) * 2019-10-03 2021-04-08 Mitsubishi Electric Corporation Method and system for trajectory optimization for nonlinear robotic systems with geometric constraints
CN114502335A (en) * 2019-10-03 2022-05-13 三菱电机株式会社 Method and system for trajectory optimization of a non-linear robotic system with geometric constraints
CN111185923A (en) * 2020-01-14 2020-05-22 深圳众为兴技术股份有限公司 Robot control device and control method
US20210308862A1 (en) * 2020-04-03 2021-10-07 Fanuc Corporation Fast robot motion optimization with distance field
CN115778445A (en) * 2022-12-07 2023-03-14 佗道医疗科技有限公司 Laser guidance verification method for tail end of mechanical arm
CN115922728A (en) * 2023-01-03 2023-04-07 之江实验室 Robot pointing motion control method, device, electronic device and storage medium

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
廖波;尚建忠;ERNEST APPLETON;王晓明;: "基于遍历迭代法的五自由度拟人手臂逆运动学求解", 机械设计与研究, no. 05 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN118305813A (en) * 2024-06-12 2024-07-09 苏州远昌智能科技有限公司 Teleoperation control method and system for humanoid robot

Also Published As

Publication number Publication date
CN116141341B (en) 2023-08-08

Similar Documents

Publication Publication Date Title
CN107490965B (en) Multi-constraint trajectory planning method for space free floating mechanical arm
Dixon et al. Adaptive tracking control of a wheeled mobile robot via an uncalibrated camera system
CN109159151B (en) Mechanical arm space trajectory tracking dynamic compensation method and system
CN111360827B (en) Visual servo switching control method and system
CN114952868B (en) 7-degree-of-freedom SRS (sounding reference Signal) type mechanical arm control method and device and piano playing robot
CN111553239B (en) Robot joint vision servo control method, terminal equipment and storage medium
CN113715016B (en) Robot grabbing method, system, device and medium based on 3D vision
CN110850807B (en) Singular point avoiding method, device, equipment and medium
CN116141341B (en) Method for realizing pointing action of five-degree-of-freedom mechanical arm meeting Cartesian space constraint
CN114670191B (en) Seven-degree-of-freedom elbow offset mechanical arm track planning method
CN108098777B (en) Redundant manipulator moment layer repetitive motion control method
CN117428791B (en) Inverse kinematics solving method and system for shoulder four-axis rehabilitation robot
CN113199481B (en) Robot motion control method, device, electronic apparatus, and medium
CN115213905A (en) Redundant manipulator pose control method and system and robot
CN114654466A (en) Automatic calibration method, device, system, electronic equipment and storage medium
CN116985147B (en) Mechanical arm inverse kinematics solving method and device
CN113814988A (en) 7-degree-of-freedom SRS type mechanical arm inverse solution analysis method and device and electronic equipment
CN109542094B (en) Mobile robot vision stabilization control without desired images
Kase et al. Visual servoing of the manipulator using the stereo vision
CN117340879A (en) Industrial machine ginseng number identification method and system based on graph optimization model
CN116604557A (en) Double-arm robot cooperative collision avoidance planning optimization method in unstructured environment
Mangkusasmito et al. Visual servo strategies using linear quadratic Gaussian (LQG) for Yaw-Pitch camera platform
Huang et al. Telepresence augmentation for visual and haptic guided immersive teleoperation of industrial manipulator
Bae et al. A dynamic visual servoing of robot manipulator with eye-in-hand camera
CN114967441A (en) Networked incomplete constraint multi-robot grouping consistent tracking control method, micro-control unit and control system

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