CN116141341B - 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 PDFInfo
- Publication number
- CN116141341B CN116141341B CN202310432715.3A CN202310432715A CN116141341B CN 116141341 B CN116141341 B CN 116141341B CN 202310432715 A CN202310432715 A CN 202310432715A CN 116141341 B CN116141341 B CN 116141341B
- 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.)
- Active
Links
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
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:
，
wherein the method comprises the steps ofThree joint angles of the shoulder, the elbow and the wrist, respectively,/>Is thatCorresponding weight coefficient, ++>For a shift of the target point to be pointed to in the Z-axis direction relative to the origin of the world coordinate system,、/>and the same is done; />Transformation matrix representing wrist coordinate system relative to world coordinate system, < >>Representation of the wrist X-axis in world coordinate system,/->Representation of the wrist Y-axis in world coordinate system,/->Representation of the wrist Z-axis in world coordinate system,/->Representing the origin offset of the wrist coordinate system relative to the world coordinate system,、/>、/>for the position of the origin of the wrist coordinate system relative to the origin of the world coordinate system,/->Representing the origin offset of the target point to be pointed to with respect to the relative world coordinate system,/->、/>、/>、/>、、/>、/>And the same is done; />The ith optimization result for the jth joint,/->For the j-th joint, for the i-1 st optimization result to be directed to the target point,/th joint>For Cartesian space constraints for the environment, +.>For Cartesian space constraints for the robot body, +.>、/>Respectively->And->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;
the homogeneous transformation (denoted as T) of each joint corresponding to the last joint coordinate system is generally obtained by DH modeling and rotation,the wrist joint is transformed uniformly corresponding to the world coordinate system.
Here will beSummarizing together, representing homogeneous transformation of the shoulder with respect to the robot coordinate system as passing +.>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:
in the method, in the process of the invention,representing the position of the robot shoulder joint corresponding to the world coordinate system,representing the position of the robot elbow joint corresponding to the world coordinate system,/->Representing the position of the robot wrist joint corresponding to the world coordinate system,/->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:
in the method, in the process of the invention,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:
，1≤j≤5
in the method, in the process of the invention,minimum value of j-th joint variable of 5R mechanical arm, < >>For the actual value of the jth joint variable of the 5R mechanical arm,>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:
，
wherein the method comprises the steps ofThree joint angles of the shoulder, the elbow and the wrist, respectively,/>Is thatCorresponding weight coefficient, ++>For a shift of the target point to be pointed to in the Z-axis direction relative to the origin of the world coordinate system,、/>and the same is done; />Transformation matrix representing wrist coordinate system relative to world coordinate system, < >>Representation of the wrist X-axis in world coordinate system,/->Representation of the wrist Y-axis in world coordinate system,/->Representation of the wrist Z-axis in world coordinate system,/->Representing the origin offset of the wrist coordinate system relative to the world coordinate system,、/>、/>for the position of the origin of the wrist coordinate system relative to the origin of the world coordinate system,/->Representing the origin offset of the target point to be pointed to with respect to the relative world coordinate system,/->、/>、/>、/>、、/>、/>And the same is done; />The ith optimization result for the jth joint,/->For the j-th joint, for the i-1 st optimization result to be directed to the target point,/th joint>For Cartesian space constraints for the environment, +.>For Cartesian space constraints for the robot body, +.>、/>Respectively->And->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 Z, 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 minimizing the deviation between the joint variables when two or more points to be pointed to appear, T representing the homogeneous transformation matrix of the wrist coordinate system relative to the world coordinate system,representation of the wrist X-axis in world coordinate system,/->Representation of the wrist Y-axis in world coordinate system,/->Representing the Z-axis of the wrist in the world coordinate systemThe following expression->Representing the origin offset of the wrist coordinate system relative to the world coordinate system,/->、/>、/>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. />The ith optimization result for the jth joint,/->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 on the track sequence of the target point to be pointed to in non-real time, the i-th time represents the optimization result for the i-th point in the sequence, i-1 is the optimization result for the i-1-th point in the sequence, the deviation between the two adjacent optimization solutions is minimized in the optimization process, for example, the deviation between the joint angles output by the first pointing point and the joint angles of the second pointing point is minimized in the optimization solution process; 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:
in the method, in the process of the invention,representing the current angle of connection +.>(corresponding time->) And the next angle->(corresponding time->) About time->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
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 direction of a wrist rotating shaft and the collineation of the target point to be pointed as main optimization targets, and converting the Cartesian space constraint on a 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:
，
wherein the method comprises the steps ofThree joint angles of the shoulder, the elbow and the wrist, respectively,/>Is thatf i Corresponding weight coefficients, i=1, 2,3,4,5,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, +.>、/>And the same is done; />Transformation matrix representing wrist coordinate system relative to world coordinate system, < >>Representation of the wrist X-axis in world coordinate system,/->Representation of the wrist Y-axis in world coordinate system,/->Representation of the wrist Z-axis in world coordinate system,/->Representing the origin offset of the wrist coordinate system relative to the world coordinate system,、/>、/>for the position of the origin of the wrist coordinate system relative to the origin of the world coordinate system,/->Representing the origin offset of the target point to be pointed to with respect to the relative world coordinate system,/->、/>、/>、/>、、/>、/>And the same is done; />The ith optimization result for the jth joint,/->For the j-th joint, for the i-1 st optimization result to be directed to the target point,/th joint>For Cartesian space constraints for the environment, +.>For Cartesian space constraints for the robot body, +.>、/>Respectively->And->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 limiting 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 the direction of a wrist rotating shaft and the collineation of 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.
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 CN116141341A (en) | 2023-05-23 |
| CN116141341B true 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) |
Citations (9)
| 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 |
| 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 |
| 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 |
| 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 |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9044857B2 (en) * | 2012-02-14 | 2015-06-02 | Jerry Neal 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 |
| WO2021041223A1 (en) * | 2019-08-23 | 2021-03-04 | Realtime Robotics, Inc. | Motion planning for robots to optimize velocity while maintaining limits on acceleration and jerk |
| US11724387B2 (en) * | 2020-04-03 | 2023-08-15 | Fanuc Corporation | Fast robot motion optimization with distance field |
-
2023
- 2023-04-21 CN CN202310432715.3A patent/CN116141341B/en active Active
Patent Citations (10)
| 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 |
| 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 |
| 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 |
| 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)
| Title |
|---|
| 基于遍历迭代法的五自由度拟人手臂逆运动学求解;廖波;尚建忠;Ernest Appleton;王晓明;;机械设计与研究(05);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116141341A (en) | 2023-05-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Dixon et al. | Adaptive tracking control of a wheeled mobile robot via an uncalibrated camera system | |
| CN107490965B (en) | Multi-constraint trajectory planning method for space free floating mechanical arm | |
| CN111360827B (en) | Visual servo switching control method and system | |
| CN109159151B (en) | Mechanical arm space trajectory tracking dynamic compensation method and system | |
| CN107214702B (en) | Planning method and system for determining robot track by using virtual reality handle | |
| CN111553239B (en) | Robot joint vision servo control method, terminal equipment and storage medium | |
| CN114952868B (en) | 7-degree-of-freedom SRS (sounding reference Signal) type mechanical arm control method and device and piano playing robot | |
| CN114670191B (en) | Seven-degree-of-freedom elbow offset mechanical arm track planning method | |
| CN113715016B (en) | Robot grabbing method, system, device and medium based on 3D vision | |
| CN108098777B (en) | Redundant manipulator moment layer repetitive motion control method | |
| CN116276966A (en) | Mobile operation robot whole body reaction planning control method based on quadratic programming | |
| Li et al. | Hybrid uncalibrated visual servoing control of harvesting robots with RGB-D cameras | |
| CN113199481B (en) | Robot motion control method, device, electronic apparatus, and medium | |
| CN117428791B (en) | Inverse kinematics solving method and system for shoulder four-axis rehabilitation robot | |
| CN114654466A (en) | Automatic calibration method, device, system, electronic equipment and storage medium | |
| CN116141341B (en) | Method for realizing pointing action of five-degree-of-freedom mechanical arm meeting Cartesian space constraint | |
| Kase et al. | Visual servoing of the manipulator using the stereo vision | |
| Yang et al. | Optimal configuration for mobile robotic grinding of large complex components based on redundant parameters | |
| 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 | |
| CN109542094B (en) | Mobile robot vision stabilization control without desired images | |
| Bae et al. | A dynamic visual servoing of robot manipulator with eye-in-hand camera | |
| Lang et al. | Visual servoing with LQR control for mobile robots | |
| 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 |