CN116922384A - Mechanical arm control method, device, equipment and storage medium - Google Patents
Mechanical arm control method, device, equipment and storage medium Download PDFInfo
- Publication number
- CN116922384A CN116922384A CN202310907633.XA CN202310907633A CN116922384A CN 116922384 A CN116922384 A CN 116922384A CN 202310907633 A CN202310907633 A CN 202310907633A CN 116922384 A CN116922384 A CN 116922384A
- Authority
- CN
- China
- Prior art keywords
- joint
- axis
- coordinate system
- pose
- mechanical arm
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 52
- 239000013598 vector Substances 0.000 claims description 16
- 238000004364 calculation method Methods 0.000 claims description 10
- 238000006243 chemical reaction Methods 0.000 claims description 10
- 238000004590 computer program Methods 0.000 claims description 9
- 230000008569 process Effects 0.000 description 13
- 238000010586 diagram Methods 0.000 description 5
- 238000004422 calculation algorithm Methods 0.000 description 4
- 230000009471 action Effects 0.000 description 3
- 230000007547 defect Effects 0.000 description 2
- 238000006073 displacement reaction Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000010606 normalization Methods 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
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
Abstract
The disclosure relates to a method, a device, equipment and a storage medium for controlling a mechanical arm, wherein the method comprises the following steps: inputting a target pose; taking the terminal flange of the mechanical arm towards a preset direction as a constraint condition, and determining a target plane where a rotation shaft in an nth joint coordinate system corresponding to the nth joint shaft is positioned by combining a mechanical size offset relation between the terminal flange and the nth joint shaft of the mechanical arm; according to the target pose, solving the pose of the N joint coordinate system when the rotating shaft meets the target plane; and determining joint angles of all joint shafts in the mechanical arm according to the pose of the N joint coordinate system and the mechanical size offset relation among all joint shafts in the mechanical arm so as to control the mechanical arm through the joint angles. According to the technical scheme disclosed by the invention, a motion control solving scheme with redundant freedom mechanical arms can be realized.
Description
Technical Field
The disclosure relates to the technical field of data processing, and in particular relates to a method, a device, equipment and a storage medium for controlling a mechanical arm.
Background
In the application scenario of medical surgical robots, the effective working space of the robotic arm is critical. The existing cooperative mechanical arm can increase the effective working space of the mechanical arm by adding a redundant shaft on the basis of the existing degree of freedom, such as a mechanical arm with 7 degrees of freedom, but is subjected to cooperative demands of force control and the like, the size of a connecting rod of the mechanical arm with 7 degrees of freedom and the movement range of a joint are small, and the situation that the mechanical arm cannot be reached still occurs for the target pose of certain application scenes, so that richer reachable poses can be realized by adding the redundant degree of freedom at the tail end, and the increase of the redundant degree of freedom brings obvious challenges to the motion control algorithm of the mechanical arm.
At present, a motion control scheme for a mechanical arm with multiple degrees of freedom exists, and for the motion control scheme for a mechanical arm with a redundant degree of freedom configuration is further added on the basis of the mechanical arm with multiple degrees of freedom, at present, a unified and efficient kinematic inverse solution solving algorithm is not available, specifically, the solution problem cannot be solved in a solution analysis mode, and when the solution is achieved in a numerical solution mode, the problems that the iteration times are uncontrollable and the effective solution cannot be guaranteed exist.
Disclosure of Invention
In order to solve the technical problems, the disclosure provides a method, a device, equipment and a storage medium for controlling a mechanical arm.
In a first aspect, an embodiment of the present disclosure provides a method for controlling a mechanical arm, which is applied to a mechanical arm having a redundant degree of freedom, the method including:
inputting a target pose;
taking the terminal flange of the mechanical arm towards a preset direction as a constraint condition, and determining a target plane where a rotation shaft in an nth joint coordinate system corresponding to the nth joint shaft is positioned by combining a mechanical size offset relation between the terminal flange and the nth joint shaft of the mechanical arm; wherein N is the number of degrees of freedom of the mechanical arm, and the end flange is the N-1 joint shaft of the mechanical arm;
according to the target pose, solving the pose of the N joint coordinate system when the rotating shaft meets the target plane;
and determining joint angles of all joint shafts in the mechanical arm according to the pose of the N joint coordinate system and the mechanical size offset relation among all joint shafts in the mechanical arm so as to control the mechanical arm through the joint angles.
In a second aspect, an embodiment of the present disclosure provides a robot arm control device applied to a robot arm having a redundant degree of freedom, the device including:
the input module is used for inputting the pose of the target;
the determining module is used for determining a target plane where a rotation shaft is located in an nth joint coordinate system corresponding to the nth joint shaft by taking the tail end flange of the mechanical arm towards a preset direction as a constraint condition and combining a mechanical size offset relation between the tail end flange and the nth joint shaft of the mechanical arm; wherein N is the number of degrees of freedom of the mechanical arm, and the end flange is the N-1 joint shaft of the mechanical arm;
the processing module is used for solving the pose of the N joint coordinate system when the rotating shaft meets the target plane according to the target pose;
and the control module is used for determining the joint angles of all the joint shafts in the mechanical arm according to the pose of the N joint coordinate system and the mechanical dimensional offset relation among all the joint shafts in the mechanical arm so as to control the mechanical arm through the joint angles.
In a third aspect, an embodiment of the present disclosure provides an electronic device, including: a processor; a memory for storing the processor-executable instructions; the processor is configured to read the executable instruction from the memory, and execute the instruction to implement the method for controlling a mechanical arm according to the first aspect.
In a fourth aspect, an embodiment of the present disclosure provides a computer readable storage medium storing a computer program, where the computer program is executed by a processor to implement the method for controlling a robotic arm according to the first aspect.
Compared with the prior art, the technical scheme provided by the embodiment of the disclosure has the following advantages: the mechanical arm motion control solving scheme with redundant freedom degree can be realized by taking the terminal flange of the mechanical arm towards the preset direction as a constraint condition and combining the input target pose and the known mechanical size offset relation, for example, the motion control solving scheme of the mechanical arm with 7+1 freedom degree is realized, the whole solving process is stable and controllable, the excessive numerical iteration process is not involved, the defect that the prior art has no effective analytic solution or directly carries out solving through numerical iteration is overcome, the redundant freedom degree is supported to be increased on the basis of the mechanical arm with multiple degrees of freedom, the effective working range of the mechanical arm is increased to reach the target pose with a larger range, the use scene of the medical robot is further expanded, and the medical robot is ensured to have a larger working range and safety in work.
Drawings
The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the disclosure and together with the description, serve to explain the principles of the disclosure.
In order to more clearly illustrate the embodiments of the present disclosure or the solutions in the prior art, the drawings that are required for the description of the embodiments or the prior art will be briefly described below, and it will be obvious to those skilled in the art that other drawings can be obtained from these drawings without inventive effort.
Fig. 1 is a schematic flow chart of a method for controlling a mechanical arm according to an embodiment of the disclosure;
fig. 2 is a schematic structural diagram of a mechanical arm according to an embodiment of the disclosure;
FIG. 3 is a schematic diagram of another method for controlling a robotic arm according to an embodiment of the disclosure;
FIG. 4 is a schematic view of an eighth joint coordinate system pose provided by embodiments of the present disclosure;
fig. 5 is a schematic structural diagram of a mechanical arm control device according to an embodiment of the disclosure.
Detailed Description
In order that the above objects, features and advantages of the present disclosure may be more clearly understood, a further description of aspects of the present disclosure will be provided below. It should be noted that, without conflict, the embodiments of the present disclosure and features in the embodiments may be combined with each other.
In the following description, numerous specific details are set forth in order to provide a thorough understanding of the present disclosure, but the present disclosure may be practiced otherwise than as described herein; it will be apparent that the embodiments in the specification are only some, but not all, embodiments of the disclosure.
Fig. 1 is a schematic flow chart of a method for controlling a mechanical arm according to an embodiment of the present disclosure, where the method provided by the embodiment of the present disclosure may be executed by a mechanical arm control device, and the device may be implemented by software and/or hardware and may be integrated on any electronic device with computing capability.
As shown in fig. 1, the method for controlling a mechanical arm provided in the embodiment of the disclosure may include:
step 101, inputting the pose of the target.
The method of the embodiment of the disclosure may be used for motion control of an N-degree-of-freedom mechanical arm having redundant degrees of freedom, for example, may be used for motion control of an 8 (7+1) -degree-of-freedom mechanical arm, and n=8 is taken as an example for illustration. The 8-degree-of-freedom mechanical arm is a configuration mechanical arm for adding 8 th axis redundancy degree of freedom to the 7-degree-of-freedom mechanical arm.
The 7+1 degree of freedom mechanical arm in this embodiment includes a base, a first joint shaft, a second joint shaft, a third joint shaft, a fourth joint shaft, a fifth joint shaft, a sixth joint shaft, a seventh joint shaft, an eighth joint shaft, and an end tool in this order.
As an example, as shown in fig. 2, the target pose is the O point in fig. 2; for the robot base, point B in fig. 2 indicates the position of the base coordinate system of the robot; the end flange of the mechanical arm is a seventh joint shaft of the mechanical arm and corresponds to a seventh joint coordinate system, and in fig. 2, the point E represents the position of the seventh joint coordinate system; the sixth joint axis is the previous joint axis of the end flange and corresponds to the sixth joint coordinate system, and in fig. 2, the S point represents the position of the sixth joint coordinate system; mounting an eighth joint shaft on a flange at the tail end of the 7-degree-of-freedom mechanical arm to realize 7+1 redundant degrees of freedom, wherein for an eighth coordinate system of the eighth joint shaft, an R point in FIG. 2 represents the position of the eighth joint coordinate system; and the tail end tool pose represents the position and the pose of the tail end tool operating point carried by the mechanical arm, as shown by a T point in fig. 2, and when the tail end tool reaches the target pose, the O point coincides with the T point pose.
Representing the conversion relation of the target pose coordinate system relative to the mechanical arm base coordinate system, and +.>Representing the transformation relation of the end tool coordinate system relative to the manipulator base coordinate system, +.>Representing the transformation relation of the end tool coordinate system with respect to the joint coordinate system of the eighth joint axis, +.>Representing the conversion relation of the joint coordinate system of the eighth joint axis relative to the flange coordinate system of the mechanical arm, +.>And the conversion relation of the joint coordinate system of the flange coordinate system of the mechanical arm relative to the sixth joint axis is shown.
In this embodiment, the target pose represents the final position and pose to be achieved by the end tool carried by the manipulator, and the target pose may be input by the user through inputSo that the joint angles of the mechanical arm are outputted through subsequent calculation.
And 102, determining a target plane where a rotation shaft is located in an N-th joint coordinate system corresponding to the N-th joint shaft by taking the orientation of the tail end flange of the mechanical arm to a preset direction as a constraint condition and combining a mechanical size offset relation between the tail end flange and the N-th joint shaft of the mechanical arm.
In this embodiment, when solving each joint angle of the mechanical arm, a constraint condition that the end flange of the mechanical arm faces a preset direction is added, where the preset direction may be a preset fixed direction, for example, the preset direction is a vertical direction, and it should be noted that the preset direction is not limited to the vertical direction, and may be a horizontal direction, a direction with a fixed included angle with the vertical direction or the horizontal direction.
For the robot arm, a mechanical dimensional displacement relationship between joint axes of the robot arm is known, which is used to indicate information such as an angle between the joint axes, and further, according to the mechanical dimensional displacement relationship between the end flange and the eighth joint axis, it is possible to determine a plane in which the rotation axis is located in the eighth joint coordinate system when the constraint condition of orientation of the end flange is satisfied.
As an example, when the preset direction is the horizontal direction and the mechanical dimension offset relationship between the end flange and the eighth joint axis is a right angle, if the end flange of the mechanical arm faces the horizontal direction, the rotation axis of the eighth joint coordinate system should be in a vertical plane.
And 103, solving the pose of the N joint coordinate system when the rotation axis meets the target plane according to the target pose.
In the present embodiment, according to the input target pose, the other coordinate axis other than the rotation axis in the eighth joint coordinate system can be determined.
As an example, a mechanical dimensional offset relationship between the end tool of the robot arm and the eighth joint axis is known, and since the O-point coincides with the T-point pose when the end tool reaches the target pose, from the mechanical dimensional offset relationship between the target pose, the end tool of the robot arm, and the eighth joint axis, a conversion relationship description between the eighth joint coordinate system and the base coordinate system of the robot arm when the target pose is satisfied can be determinedFurthermore, the pose of the eighth joint coordinate system can be solved by using the conversion relationship between the eighth joint coordinate system and the base coordinate system of the robot arm and the constraint condition that the rotation axis is located on the target plane.
And 104, determining joint angles of all joint shafts in the mechanical arm according to the pose of the N joint coordinate system and the mechanical size offset relation among all joint shafts in the mechanical arm so as to control the mechanical arm through the joint angles.
In this embodiment, a mechanical dimensional offset relationship between each joint axis in the mechanical arm is known, including a mechanical dimensional offset relationship between the eighth joint axis and the seventh joint axis, a mechanical dimensional offset relationship between the seventh joint axis and the sixth joint axis, and the like, and joint angles of the eighth joint axis to the first joint axis are calculated one by one based on the calculated pose of the eighth joint coordinate system, so as to perform motion control on the mechanical arm.
As an example, the joint angle of the eighth joint axis can be obtained from the pose of the eighth joint coordinate system, and the pose of the end flange can be obtained from the pose of the eighth joint coordinate system, the mechanical dimensional offset relationship between the end flange and the eighth joint axis. And inputting the pose of the end flange into a preset inverse solution calculation model to obtain joint angles of the first joint axis to the seventh joint axis in the mechanical arm, wherein the inverse solution calculation model is an inverse solution calculation method of the existing mechanical arm with 7 degrees of freedom, for example, an algorithm interface provided by an existing manufacturer can be adopted.
It should be noted that, the above description is given taking the 8-degree-of-freedom mechanical arm as an example, and the present solution has the best application effect for the 8-degree-of-freedom mechanical arm, but the method of the embodiment is not limited to the 8-degree-of-freedom mechanical arm, and other mechanical arms with redundant degrees of freedom may also be applied.
According to the technical scheme of the embodiment of the disclosure, by taking the terminal flange of the mechanical arm towards the preset direction as a constraint condition and combining the input target pose and the known mechanical dimension offset relation, the mechanical arm motion control solving scheme with redundant degrees of freedom can be realized, for example, the motion control solving scheme of the mechanical arm with 7+1 degrees of freedom is realized, the whole solving process is stable and controllable, the excessive numerical iteration process is not involved, the defect that the prior art does not have effective analytic solution or directly carries out solving through numerical iteration is overcome, the redundant degrees of freedom are increased on the basis of the mechanical arm with multiple degrees of freedom, the effective working range of the mechanical arm is increased to reach the target pose with a larger range, the use scene of the medical robot is further expanded, and the medical robot is ensured to have a larger working range and safety in work.
Based on the above embodiments, fig. 3 is a schematic diagram of another method for controlling a mechanical arm according to an embodiment of the disclosure, as shown in fig. 3, where the method includes:
in step 301, the target pose is input, and when the mechanical dimension offset relationship between the end flange and the eighth joint axis is right angle, it is determined that the X axis in the eighth joint coordinate system should be in a horizontal plane with the end flange of the mechanical arm being vertically downward as a constraint condition.
In this embodiment, if the target posture is strictly ensured to be unchanged, it cannot be determined that the end flange of the mechanical arm can be kept vertically downward, and therefore, when the constraint condition that the end flange is vertically downward is satisfied by combining the mechanical dimensional offset relationship between the end flange and the eighth joint axis to be a right angle, the X axis in the eighth joint coordinate system should be on a horizontal plane.
Step 302, determining the Z-axis direction of the eighth joint coordinate system according to the target pose, keeping the Z-axis direction of the eighth joint coordinate system unchanged, and solving the X-axis and the Y-axis of the eighth joint coordinate system to obtain a first candidate pose and a second candidate pose of the eighth joint coordinate system.
In this embodiment, according to the target pose, the Z axis direction of the eighth joint coordinate system, that is, the Z axis direction of the eighth joint coordinate system, can be determined as a known input, and at this time, the Z axis direction of the eighth joint coordinate system is kept unchanged, and rotation is performed based on the Z axis, so that the X axis is on the horizontal plane, for example, referring to fig. 4, the Z axis of the R coordinate system is rotated to make the X axis on the P plane, and at this time, there are two cases that a first candidate solution and a second candidate solution of the X axis can be obtained, where the first candidate solution and the second candidate solution of the X axis both satisfy that the X axis is on the target plane, and are denoted by X' and X ", respectively.
In this embodiment, the Y-axis of the eighth joint coordinate system is solved according to the first candidate solution of the X-axis and the known Z-axis to obtain the first candidate pose of the eighth joint coordinate system, and the Y-axis of the eighth joint coordinate system is solved according to the second candidate solution of the X-axis and the known Z-axis to serve as the second candidate pose of the eighth joint coordinate system.
The solving process is described below.
Setting the X direction of the X axis as a preset value according to the direction corresponding to the first candidate solution of the X axis, setting the Z direction of the X axis as zero, calculating the Y direction value of the X axis according to a formula that the dot product of two orthogonal vectors is zero by combining the values of the Z axis to obtain the values of the X axis, and carrying out cross multiplication according to the values of the X axis and the values of the Z axis to obtain the values of the Y axis to generate the first candidate pose. And setting the X direction of the X axis as a preset value according to the direction corresponding to the second candidate solution of the X axis, setting the Z direction of the X axis as zero, calculating the Y direction value of the X axis according to a formula that the dot product of two orthogonal vectors is zero by combining the values of the Z axis to obtain the values of the X axis, and carrying out cross multiplication according to the values of the X axis and the values of the Z axis to obtain the values of the Y axis to generate the second candidate pose.
Taking a candidate solution X 'of the X axis in the eighth joint coordinate system as an example, in the calculation process, the vector representation of X' can be calculated by using the basic principle that the dot product of two orthogonal vectors is zero. From vector angle calculation, assuming that the coordinates of the origin of the R coordinate system are (0, 0), assuming that the X-direction value of the X ' vector is 1, and knowing that the Z-direction value of the X ' vector is 0, the X ' vector can be expressed as (1, Y, 0), the Y-direction value of the X ' vector can be obtained by a formula in which the dot product of two orthogonal vectors is zero, then normalization processing is performed on the obtained (1, Y, 0) to obtain a unitized X ' vector, and Y ' vector is obtained by cross multiplication of the Z vector and the X ' vector, thereby obtaining the pose of the 8 th axis redundancy degree of freedom R which simultaneously satisfies the rotation axis of the eighth joint coordinate system in a horizontal plane and the Z direction of which is consistent with the target pose.
Reference may be made to the above examples for the case of X ", and no further description is given here.
Step 303, according to the first candidate pose, the second candidate pose and the mechanical dimensional offset relation between the joint axes in the mechanical arm, respectively determining a first candidate joint angle combination and a second candidate joint angle combination of the joint axes in the mechanical arm, so as to determine the joint angle combination meeting the joint limit from the candidate joint angle combinations, and using the joint angle combination as the joint angle of the joint axes in the mechanical arm.
In this embodiment, the first candidate joint angle combination and the second candidate joint angle combination are compared with preset joint limits respectively, and the joint angle combination satisfying the joint limits is determined as the joint angle of each joint axis in the mechanical arm. The joint limit can be set according to actual needs and is used for limiting the range of the joint angle.
As an example, for X ', the joint angle of the eighth joint axis may be determined by the first candidate pose, and the pose of the seventh joint axis of the robot arm may be calculated according to the mechanical dimensional offset relationship among the first candidate pose, the eighth joint axis and the seventh joint axis, further, the joint angle combination Q ' may be obtained by performing inverse solution calculation through the inverse solution interface provided by the existing 7-degree-of-freedom robot arm manufacturer, the calculation may be repeated for X "to obtain the joint angle combination Q", the Q ' and Q "may be compared, if the joint limit is exceeded, and if the joint limit is exceeded, the joint limit is discarded, and if the joint limit is within the joint limit, one joint is optionally controlled as a target for finally achieving the movement of each joint of the robot arm, thereby, the input is during the control processNamely, the conversion relation of the target pose coordinate system relative to the base coordinate system is known under the conditions that the Z direction of the flange coordinate system at the tail end of the mechanical arm is vertically downward, the mechanical dimension offset relation between the tail end tool and the eighth joint axis and the mechanical dimension offset relation between the eighth joint axis and the seventh joint axis are output as the joint angles of the mechanical arm with 7+1 degrees of freedom.
According to the embodiment of the disclosure, the constraint that the flange at the tail end of the mechanical arm is vertically downward is added, so that a motion control solution scheme of the mechanical arm with 7+1 degrees of freedom is realized, the final effect that a tool carried by the mechanical arm system reaches a target pose is not influenced, an algorithm process is designed in a mode of taking the target as a guide in the scheme, the purpose of efficiently solving the inverse kinematics solution is finally achieved, the inverse kinematics solution process of the whole redundant mechanical arm system is split into two parts, namely the target pose, the flange at the tail end and the flange at the tail end, and the base, the inverse kinematics solution process is simplified, and the whole inverse kinematics solution process is stable and controllable.
Fig. 5 is a schematic structural diagram of a mechanical arm control device according to an embodiment of the disclosure, where, as shown in fig. 5, the mechanical arm control device includes: an input module 51, a determination module 52, a processing module 53, a control module 54.
Wherein the input module 51 is configured to input a target pose.
The determining module 52 is configured to determine, with a constraint condition that a terminal flange of the mechanical arm faces a preset direction, a target plane where a rotation axis in an nth joint coordinate system corresponding to the nth joint axis is located in combination with a mechanical dimension offset relationship between the terminal flange and the nth joint axis of the mechanical arm; n is the number of degrees of freedom of the mechanical arm, and the tail end flange is the N-1 joint shaft of the mechanical arm.
And the processing module 53 is used for solving the pose of the nth joint coordinate system when the rotation axis meets the target plane according to the target pose.
And the control module 54 is configured to determine a joint angle of each joint axis in the mechanical arm according to the pose of the nth joint coordinate system and the mechanical dimensional offset relationship between each joint axis in the mechanical arm, so as to control the mechanical arm through the joint angle.
In one embodiment of the present disclosure, the preset direction is vertically downward, and the determining module 52 is specifically configured to: and when the mechanical size offset relation between the tail end flange and the N joint shaft of the mechanical arm is right angle, determining that the rotating shaft is in a horizontal plane.
In one embodiment of the present disclosure, the processing module 53 is specifically configured to: according to the target pose, the mechanical dimensional offset relation between the tail end tool of the mechanical arm and the N joint shaft, determining the conversion relation between the N joint coordinate system and the base coordinate system of the mechanical arm; and solving the pose of the N joint coordinate system by taking the conversion relation between the N joint coordinate system and the base coordinate system of the mechanical arm and the constraint condition that the rotation axis is positioned on the target plane.
In one embodiment of the present disclosure, the rotation axis is the X-axis, and the processing module 53 is specifically configured to: according to the target pose, determining the Z-axis direction of an N joint coordinate system; the Z-axis direction of an N-th joint coordinate system is kept unchanged, and a first candidate solution and a second candidate solution of an X-axis are solved, wherein the first candidate solution and the second candidate solution of the X-axis meet the condition that the X-axis is positioned on a target plane; according to the first candidate solution of the X axis and the Z axis, solving the Y axis of the N joint coordinate system to serve as the first candidate pose of the N joint coordinate system; and according to the second candidate solution of the X axis and the Z axis, solving the Y axis of the N joint coordinate system to serve as the second candidate pose of the N joint coordinate system.
In one embodiment of the present disclosure, the processing module 53 is specifically configured to: setting the X direction of the X axis as a preset value, setting the Z direction of the X axis as zero according to the direction corresponding to the first candidate solution of the X axis, and calculating the Y direction value of the X axis according to a formula that the dot product of two orthogonal vectors is zero by combining the values of the Z axis to obtain the values of the X axis in all directions; and carrying out cross multiplication according to the X-axis direction values and the Z-axis direction values to obtain Y-axis direction values so as to generate first candidate poses.
In one embodiment of the present disclosure, the control module 54 is specifically configured to: according to the first candidate pose, the second candidate pose and the mechanical size offset relation between the joint shafts in the mechanical arm, respectively determining a first candidate joint angle combination and a second candidate joint angle combination of the joint shafts in the mechanical arm; and respectively comparing the first candidate joint angle combination and the second candidate joint angle combination with preset joint limit, and determining the joint angle combination meeting the joint limit as the joint angle of each joint shaft in the mechanical arm.
In one embodiment of the present disclosure, the control module 54 is specifically configured to: according to the pose of the N joint coordinate system, determining the joint angle of the N joint axis; obtaining the pose of the tail end flange according to the pose of the N joint coordinate system and the mechanical dimensional offset relation between the tail end flange and the N joint shaft; inputting the pose of the end flange into a preset inverse solution to obtain a calculation model, and obtaining joint angles from a first joint axis to an N-1 joint axis in the mechanical arm.
The mechanical arm control device provided by the embodiment of the disclosure can execute any mechanical arm control method provided by the embodiment of the disclosure, and has the corresponding functional modules and beneficial effects of the execution method. Details of the embodiments of the apparatus of the present disclosure that are not described in detail may refer to descriptions of any of the embodiments of the method of the present disclosure.
The embodiment of the disclosure also provides an electronic device, which comprises one or more processors and a memory. The processor may be a Central Processing Unit (CPU) or other form of processing unit having data processing and/or instruction execution capabilities, and may control other components in the electronic device to perform the desired functions. The memory may include one or more computer program products, which may include various forms of computer-readable storage media, such as volatile memory and/or non-volatile memory. Volatile memory can include, for example, random Access Memory (RAM) and/or cache memory (cache) and the like. The non-volatile memory may include, for example, read Only Memory (ROM), hard disk, flash memory, and the like. One or more computer program instructions may be stored on a computer readable storage medium and a processor may execute the program instructions to implement the methods of embodiments of the present disclosure above and/or other desired functions. Various contents such as an input signal, a signal component, a noise component, and the like may also be stored in the computer-readable storage medium.
In one example, the electronic device may further include: input devices and output devices, which are interconnected by a bus system and/or other forms of connection mechanisms. In addition, the input device may include, for example, a keyboard, a mouse, and the like. The output device may output various information including the determined distance information, direction information, etc., to the outside. The output means may include, for example, a display, speakers, a printer, and a communication network and remote output devices connected thereto, etc. In addition, the electronic device may include any other suitable components, such as a bus, input/output interface, etc., depending on the particular application.
In addition to the methods and apparatus described above, embodiments of the present disclosure may also be a computer program product comprising computer program instructions which, when executed by a processor, cause the processor to perform any of the methods provided by the embodiments of the present disclosure.
The computer program product may be written in any combination of one or more programming languages, including an object oriented programming language such as Java, C++ or the like and conventional procedural programming languages, such as the "C" programming language or similar programming languages. The program code may execute entirely on the user's computing device, partly on the user's device, as a stand-alone software package, partly on the user's computing device, partly on a remote computing device, or entirely on the remote computing device or server.
Furthermore, embodiments of the present disclosure may also be a computer-readable storage medium having stored thereon computer program instructions which, when executed by a processor, cause the processor to perform any of the methods provided by the embodiments of the present disclosure.
A computer readable storage medium may employ any combination of one or more readable media. The readable medium may be a readable signal medium or a readable storage medium. The readable storage medium may include, for example, but is not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or a combination of any of the foregoing. More specific examples (a non-exhaustive list) of the readable storage medium would include the following: an electrical connection having one or more wires, a portable disk, a hard disk, random Access Memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.
It should be noted that in this document, relational terms such as "first" and "second" and the like are used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Moreover, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising" does not exclude the presence of other like elements in a process, method, article, or apparatus that comprises the element.
The foregoing is merely a specific embodiment of the disclosure to enable one skilled in the art to understand or practice the disclosure. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the generic principles defined herein may be applied to other embodiments without departing from the spirit or scope of the disclosure. Thus, the present disclosure is not intended to be limited to the embodiments shown and described herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.
Claims (10)
1. A robot arm control method, applied to a robot arm having a redundant degree of freedom, comprising:
inputting a target pose;
taking the terminal flange of the mechanical arm towards a preset direction as a constraint condition, and determining a target plane where a rotation shaft in an nth joint coordinate system corresponding to the nth joint shaft is positioned by combining a mechanical size offset relation between the terminal flange and the nth joint shaft of the mechanical arm; wherein N is the number of degrees of freedom of the mechanical arm, and the end flange is the N-1 joint shaft of the mechanical arm;
according to the target pose, solving the pose of the N joint coordinate system when the rotating shaft meets the target plane;
and determining joint angles of all joint shafts in the mechanical arm according to the pose of the N joint coordinate system and the mechanical size offset relation among all joint shafts in the mechanical arm so as to control the mechanical arm through the joint angles.
2. The method of claim 1, wherein the predetermined direction is vertically downward, and wherein the determining a target plane in which the rotation axis in the nth joint coordinate system corresponding to the nth joint axis is located in combination with the mechanical dimensional offset relationship between the end flange and the nth joint axis of the mechanical arm comprises:
and when the mechanical size offset relation between the tail end flange and the N joint shaft of the mechanical arm is a right angle, determining that the rotating shaft is in a horizontal plane.
3. The method of claim 1, wherein said solving for the pose of the nth joint coordinate system when the rotation axis satisfies the target plane based on the target pose comprises:
determining a conversion relation between the nth joint coordinate system and a base coordinate system of the mechanical arm according to the target pose, the mechanical size offset relation between the tail end tool of the mechanical arm and the nth joint shaft;
and solving the pose of the Nth joint coordinate system by taking the conversion relation between the Nth joint coordinate system and the base coordinate system of the mechanical arm and the constraint condition that the rotating shaft is positioned on the target plane.
4. The method of claim 1, wherein the axis of rotation is an X-axis, and the solving for the pose of the nth joint coordinate system when the axis of rotation satisfies the target plane based on the target pose comprises:
according to the target pose, determining the Z-axis direction of the N joint coordinate system;
the Z-axis direction of the N-th joint coordinate system is kept unchanged, and a first candidate solution and a second candidate solution of an X-axis are solved, wherein the first candidate solution and the second candidate solution of the X-axis meet the condition that the X-axis is located on the target plane;
according to the first candidate solution of the X axis and the Z axis, solving the Y axis of the N joint coordinate system to serve as the first candidate pose of the N joint coordinate system;
and according to the second candidate solution of the X axis and the Z axis, solving the Y axis of the N joint coordinate system to serve as the second candidate pose of the N joint coordinate system.
5. The method of claim 4, wherein said solving the Y-axis of the nth joint coordinate system as the first candidate pose of the nth joint coordinate system based on the first candidate solution of the X-axis and the Z-axis comprises:
setting the X direction of the X axis as a preset value according to the direction corresponding to the first candidate solution of the X axis, setting the Z direction of the X axis as zero, and calculating the Y direction value of the X axis according to a formula that the dot product of two orthogonal vectors is zero by combining the values of the Z directions to obtain the values of the X axes;
and carrying out cross multiplication according to the X-axis direction values and the Z-axis direction values to obtain the Y-axis direction values so as to generate the first candidate pose.
6. The method of claim 4, wherein determining the joint angle of each joint axis in the robotic arm based on the pose of the nth joint coordinate system, the mechanical dimensional offset relationship between each joint axis in the robotic arm, comprises:
according to the first candidate pose, the second candidate pose and the mechanical size offset relation between the joint shafts in the mechanical arm, respectively determining a first candidate joint angle combination and a second candidate joint angle combination of the joint shafts in the mechanical arm;
and comparing the first candidate joint angle combination and the second candidate joint angle combination with preset joint limit, and determining the joint angle combination meeting the joint limit as the joint angle of each joint shaft in the mechanical arm.
7. The method of claim 1, wherein determining the joint angle of each joint axis in the robotic arm based on the pose of the nth joint coordinate system, the mechanical dimensional offset relationship between each joint axis in the robotic arm, comprises:
according to the pose of the nth joint coordinate system, determining the joint angle of the nth joint shaft;
obtaining the pose of the tail end flange according to the pose of the N joint coordinate system and the mechanical dimensional offset relation between the tail end flange and the N joint shaft;
inputting the pose of the end flange into a preset inverse solution to obtain a calculation model, and obtaining joint angles from a first joint axis to an N-1 joint axis in the mechanical arm.
8. A robot arm control device, characterized by being applied to a robot arm having a redundant degree of freedom, the device comprising:
the input module is used for inputting the pose of the target;
the determining module is used for determining a target plane where a rotation shaft is located in an nth joint coordinate system corresponding to the nth joint shaft by taking the tail end flange of the mechanical arm towards a preset direction as a constraint condition and combining a mechanical size offset relation between the tail end flange and the nth joint shaft of the mechanical arm; wherein N is the number of degrees of freedom of the mechanical arm, and the end flange is the N-1 joint shaft of the mechanical arm;
the processing module is used for solving the pose of the N joint coordinate system when the rotating shaft meets the target plane according to the target pose;
and the control module is used for determining the joint angles of all the joint shafts in the mechanical arm according to the pose of the N joint coordinate system and the mechanical dimensional offset relation among all the joint shafts in the mechanical arm so as to control the mechanical arm through the joint angles.
9. An electronic device, comprising:
a processor;
a memory for storing the processor-executable instructions;
the processor is configured to read the executable instructions from the memory and execute the instructions to implement the method for controlling a robotic arm according to any one of claims 1-7.
10. A computer readable storage medium, characterized in that the storage medium stores a computer program which, when executed by a processor, implements the robot arm control method according to any one of the preceding claims 1-7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310907633.XA CN116922384A (en) | 2023-07-21 | 2023-07-21 | Mechanical arm control method, device, equipment and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310907633.XA CN116922384A (en) | 2023-07-21 | 2023-07-21 | Mechanical arm control method, device, equipment and storage medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116922384A true CN116922384A (en) | 2023-10-24 |
Family
ID=88391998
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310907633.XA Pending CN116922384A (en) | 2023-07-21 | 2023-07-21 | Mechanical arm control method, device, equipment and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116922384A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117224241A (en) * | 2023-11-15 | 2023-12-15 | 北京云力境安科技有限公司 | Control method and related device of soft endoscope control system |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2013208671A (en) * | 2012-03-30 | 2013-10-10 | Jtekt Corp | Robot control method, robot control device, and robot control system |
| CN108481324A (en) * | 2018-04-25 | 2018-09-04 | 中国科学院合肥物质科学研究院 | A kind of inverse solution engineering and its collision detection algorithm of eight shaft multifunctionals machinery arm |
| CN110712203A (en) * | 2019-09-26 | 2020-01-21 | 苏州苏相机器人智能装备有限公司 | 7-degree-of-freedom mechanical arm inverse kinematics solving algorithm |
| CN112091979A (en) * | 2020-10-10 | 2020-12-18 | 哈尔滨工业大学 | Seven-degree-of-freedom mechanical arm limiting optimization method based on position-level inverse kinematics |
| JP2021146410A (en) * | 2020-03-17 | 2021-09-27 | アズビル株式会社 | Inverse kinematics operation unit and inverse kinematics operation method |
| CN115805587A (en) * | 2022-12-29 | 2023-03-17 | 成都卡诺普机器人技术股份有限公司 | Motion analysis method and device of seven-axis robot and electronic equipment |
| CN116038702A (en) * | 2022-12-30 | 2023-05-02 | 成都卡诺普机器人技术股份有限公司 | Seven-axis robot inverse solution method and seven-axis robot |
| CN117001662A (en) * | 2023-07-20 | 2023-11-07 | 北京纳通医用机器人科技有限公司 | Mechanical arm control method, device, equipment and storage medium |
-
2023
- 2023-07-21 CN CN202310907633.XA patent/CN116922384A/en active Pending
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2013208671A (en) * | 2012-03-30 | 2013-10-10 | Jtekt Corp | Robot control method, robot control device, and robot control system |
| CN108481324A (en) * | 2018-04-25 | 2018-09-04 | 中国科学院合肥物质科学研究院 | A kind of inverse solution engineering and its collision detection algorithm of eight shaft multifunctionals machinery arm |
| CN110712203A (en) * | 2019-09-26 | 2020-01-21 | 苏州苏相机器人智能装备有限公司 | 7-degree-of-freedom mechanical arm inverse kinematics solving algorithm |
| JP2021146410A (en) * | 2020-03-17 | 2021-09-27 | アズビル株式会社 | Inverse kinematics operation unit and inverse kinematics operation method |
| CN112091979A (en) * | 2020-10-10 | 2020-12-18 | 哈尔滨工业大学 | Seven-degree-of-freedom mechanical arm limiting optimization method based on position-level inverse kinematics |
| CN115805587A (en) * | 2022-12-29 | 2023-03-17 | 成都卡诺普机器人技术股份有限公司 | Motion analysis method and device of seven-axis robot and electronic equipment |
| CN116038702A (en) * | 2022-12-30 | 2023-05-02 | 成都卡诺普机器人技术股份有限公司 | Seven-axis robot inverse solution method and seven-axis robot |
| CN117001662A (en) * | 2023-07-20 | 2023-11-07 | 北京纳通医用机器人科技有限公司 | Mechanical arm control method, device, equipment and storage medium |
Non-Patent Citations (1)
| Title |
|---|
| 时凯飞，李瑞峰: "7自由度仿人手臂运动学研究", 哈尔滨工业大学学报, vol. 35, no. 7, 31 July 2003 (2003-07-31), pages 806 - 808 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117224241A (en) * | 2023-11-15 | 2023-12-15 | 北京云力境安科技有限公司 | Control method and related device of soft endoscope control system |
| CN117224241B (en) * | 2023-11-15 | 2024-02-02 | 北京云力境安科技有限公司 | Control method and related device of soft endoscope control system |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US20200338730A1 (en) | Trajectory planning device, trajectory planning method and program | |
| Lu et al. | Collision-free and smooth joint motion planning for six-axis industrial robots by redundancy optimization | |
| CN110370256B (en) | Robot and path planning method, device and controller thereof | |
| CN110640746B (en) | Method, system, equipment and medium for calibrating and positioning coordinate system of robot | |
| US11130227B2 (en) | Robot control device, robot, and robot system | |
| CN116922384A (en) | Mechanical arm control method, device, equipment and storage medium | |
| Xu et al. | Mobility, kinematic analysis, and dimensional optimization of new three-degrees-of-freedom parallel manipulator with actuation redundancy | |
| CN116038702B (en) | Seven-axis robot inverse solution method and seven-axis robot | |
| Ahmad et al. | A stiffness modeling methodology for simulation-driven design of haptic devices | |
| Zhang et al. | Applying a unit-consistent generalized matrix inverse for stable control of robotic systems | |
| Pan et al. | RobMach: G-Code-based off-line programming for robotic machining trajectory generation | |
| EP3978203A1 (en) | Singularity-free kinematic parameterization of soft robot manipulators | |
| CN113263496B (en) | Method for optimizing path of six-degree-of-freedom mechanical arm and computer equipment | |
| CN113814988A (en) | 7-degree-of-freedom SRS type mechanical arm inverse solution analysis method and device and electronic equipment | |
| CN117001662A (en) | Mechanical arm control method, device, equipment and storage medium | |
| CN112975947A (en) | Component pin correction method, device, equipment and storage medium | |
| JP5569953B2 (en) | Robot control system, robot control method, robot control apparatus, and program applied to high-speed and high-precision contact work | |
| CN116038660A (en) | Method and device for cooperation of robot and guide rail and electronic equipment | |
| CN115284298A (en) | Singularity avoidance method and device for robot, terminal and medium | |
| CN113021345A (en) | Method, device and equipment for controlling inverse kinematics of mechanical arm and readable storage medium | |
| Nagata et al. | Reverse and forward post processors for a robot machining system | |
| US9925668B2 (en) | Robot, control unit, and control method | |
| CN114986132B (en) | Robot flexible shaft hole assembly method and device based on visual sense | |
| CN116652972B (en) | Series robot tail end track planning method based on bidirectional greedy search algorithm | |
| EP3978209A1 (en) | Generative design techniques for soft robot manipulators |
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 |