CN113442137A - Method and device for determining joint angle of mechanical arm, storage medium and electronic equipment - Google Patents
Method and device for determining joint angle of mechanical arm, storage medium and electronic equipment Download PDFInfo
- Publication number
- CN113442137A CN113442137A CN202110706562.8A CN202110706562A CN113442137A CN 113442137 A CN113442137 A CN 113442137A CN 202110706562 A CN202110706562 A CN 202110706562A CN 113442137 A CN113442137 A CN 113442137A
- Authority
- CN
- China
- Prior art keywords
- joint
- information
- angle
- determining
- target
- 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 49
- 238000003860 storage Methods 0.000 title claims abstract description 10
- 239000011159 matrix material Substances 0.000 claims abstract description 74
- 238000005457 optimization Methods 0.000 claims abstract description 23
- 230000006870 function Effects 0.000 claims description 43
- 238000004590 computer program Methods 0.000 claims description 11
- 238000010276 construction Methods 0.000 claims description 5
- 210000001503 joint Anatomy 0.000 description 22
- 230000008569 process Effects 0.000 description 9
- 241000234314 Zingiber Species 0.000 description 7
- 235000006886 Zingiber officinale Nutrition 0.000 description 7
- 238000004364 calculation method Methods 0.000 description 7
- 238000004891 communication Methods 0.000 description 7
- 235000008397 ginger Nutrition 0.000 description 7
- 238000010586 diagram Methods 0.000 description 4
- 230000005236 sound signal Effects 0.000 description 4
- 241000282414 Homo sapiens Species 0.000 description 3
- 238000013459 approach Methods 0.000 description 3
- 238000011161 development Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 210000000707 wrist Anatomy 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007123 defense Effects 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 230000004069 differentiation Effects 0.000 description 1
- 239000012636 effector Substances 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000002035 prolonged effect Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000003466 welding Methods 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
Landscapes
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
The disclosure relates to a method and a device for determining joint angles of a mechanical arm, a storage medium and electronic equipment, which are used for accurately determining the joint angles of the mechanical arm. The determination method comprises the following steps: acquiring length information of each joint of a target mechanical arm and current angle information of each joint; determining the current position information of the tail end joint of the target mechanical arm and a Jacobian matrix of the target mechanical arm according to the length information of each joint and the current angle information of each joint; constructing a joint angle cost function according to a preset joint angle optimization condition; determining joint angle increment information by using the current position information of the tail end joint, the Jacobian matrix and the joint angle cost function; and updating the current angle information of each joint by using the joint angle increment information, and re-executing the step of determining the current position information of the tail end joint of the target mechanical arm until the position information of the tail end joint is consistent with the target position.
Description
Technical Field
The disclosure relates to the technical field of robot control, and in particular relates to a method and a device for determining a joint angle of a mechanical arm, a storage medium and an electronic device.
Background
With the rapid development of the robot technology and the acceleration of the social industrialization process, the robot gradually participates in the activities of the human beings and becomes a good helper of the human beings. The mechanical arm is also an important type of robot, and can complete various tasks such as assembly, transportation, welding and the like, and also can replace human beings to complete established tasks in dangerous and severe working environments. It is widely applied to the fields of industrial manufacturing, space exploration, national defense science and technology and the like.
In the control field of mechanical arms, the mechanical arm moves from one position to another position according to a given path, the angle of each joint needs to be calculated in real time in the whole movement process, the rotation angle of a motor is controlled to ensure that each joint in the mechanical arm moves according to the corresponding angle, and finally the mechanical arm moves to a target position to complete a task.
Disclosure of Invention
The purpose of the present disclosure is to provide a method and an apparatus for determining a joint angle of a robot arm, a storage medium, and an electronic device, so as to accurately determine each joint angle of the robot arm.
In order to achieve the above object, the present disclosure provides a method for determining a joint angle of a robot arm, including:
acquiring length information of each joint of a target mechanical arm and current angle information of each joint;
determining the current position information of the tail end joint of the target mechanical arm and a Jacobian matrix of the target mechanical arm according to the length information of each joint and the current angle information of each joint;
constructing a joint angle cost function according to a preset joint angle optimization condition;
determining joint angle increment information by using the current position information of the tail end joint, the Jacobian matrix and the joint angle cost function;
and updating the current angle information of each joint by using the joint angle increment information, and re-executing the step of determining the current position information of the tail end joint of the target mechanical arm until the position information of the tail end joint is consistent with the target position.
Optionally, the determining joint angle increment information by using the current position information of the end joint, the jacobian matrix and the joint angle cost function includes:
determining position increment information of the tail end joint according to the current position information of the tail end joint and the position information of the tail end joint determined last time;
determining an optimized gradient direction of the joint angle according to the joint angle cost function;
and determining joint angle increment information according to the position increment information, the optimized gradient direction and the Jacobian matrix.
Optionally, the determining joint angle increment information according to the position increment information, the optimized gradient direction and the jacobian matrix includes:
determining joint angle increment information by the following formula:
δq＝J+δp+(I-J+J)η
wherein δ q represents the joint angle increment information, J represents the Jacobian matrix, J+And characterizing a pseudo-inverse matrix of the Jacobian matrix, I characterizing a unit matrix of the Jacobian matrix, eta characterizing the optimized gradient direction, and δ p characterizing position increment information of the tail end joint.
Optionally, the joint angle optimization condition is that the angle of the target joint is close to the limit center of the target joint;
the constructing of the joint angle cost function according to the preset joint angle optimization condition comprises the following steps:
and constructing a joint angle cost function according to the limit center of the target joint and the current angle information of the target joint.
Optionally, the target joint is at least one of the target robot arm joints.
Optionally, the current position information of the target mechanical arm end joint is determined by:
aiming at each joint of the target mechanical arm, constructing a coordinate system of the joint under a parent coordinate system of the joint according to the length information of the joint and the current angle information of the joint;
determining a coordinate system of the tail end joint of the target mechanical arm in a world coordinate system according to the coordinate system of each joint in the parent coordinate system of each joint;
and determining the current position information of the end joint according to the coordinate system of the end joint in the world coordinate system and the length information of the end joint.
Optionally, the jacobian matrix of the target robotic arm is determined by:
constructing a current joint angle matrix according to the current angle information of each joint;
and determining the partial differential of the current position information of the tail end joint to the current joint angle matrix as the Jacobian matrix of the target mechanical arm.
A second aspect of the present disclosure provides an apparatus for determining a joint angle of a robot arm, including:
the acquisition module is used for acquiring length information of each joint of the target mechanical arm and current angle information of each joint;
the first determining module is used for determining the current position information of the tail end joint of the target mechanical arm and the Jacobian matrix of the target mechanical arm according to the length information of each joint and the current angle information of each joint;
the construction module is used for constructing a joint angle cost function according to a preset joint angle optimization condition;
the second determination module is used for determining joint angle increment information by utilizing the current position information of the tail end joint, the Jacobian matrix and the joint angle cost function;
and the execution module is used for updating the current angle information of each joint by using the joint angle increment information and re-executing the step of determining the current position information of the tail end joint of the target mechanical arm until the position information of the tail end joint is consistent with the target position.
A third aspect of the present disclosure provides a computer readable storage medium having stored thereon a computer program which, when executed by a processor, performs the steps of the method provided by the first aspect of the present disclosure.
A fourth aspect of the present disclosure provides a robot comprising:
a robotic arm, and a joint angle of the robotic arm is determined by the method provided by the first aspect of the disclosure;
a memory having a computer program stored thereon;
a processor for executing the computer program in the memory to implement the steps of the method provided by the first aspect of the present disclosure.
According to the technical scheme, the joint angle cost function is constructed according to the preset joint angle optimization condition, and then the current position information, the Jacobian matrix and the joint angle cost function of the tail end joint are comprehensively considered when the joint angle of the mechanical arm is determined. Therefore, in the process that the target mechanical arm moves to the target position, the motion angle of each joint can be ensured to meet the preset joint angle optimization condition, namely, each joint angle of the mechanical arm can be accurately determined from the infinite solution of the joint angle through the joint angle cost function, so that the target mechanical arm can move to the target position by adopting the most appropriate path.
Additional features and advantages of the disclosure will be set forth in the detailed description which follows.
Drawings
The accompanying drawings, which are included to provide a further understanding of the disclosure and are incorporated in and constitute a part of this specification, illustrate embodiments of the disclosure and together with the description serve to explain the disclosure without limiting the disclosure. In the drawings:
fig. 1 is a flowchart illustrating a method for determining a joint angle of a robot arm according to an exemplary embodiment.
FIG. 2 is a flow chart illustrating a method of determining joint angle delta information according to an exemplary embodiment.
Fig. 3 is a block diagram illustrating an apparatus for determining a joint angle of a robot arm according to an exemplary embodiment.
FIG. 4 is a block diagram illustrating a robot in accordance with an exemplary embodiment.
Detailed Description
The traditional determination of the angle value of each joint of the mechanical arm is obtained by using the coordinate position (or pose) of an end effector of the known mechanical arm through the inverse kinematics calculation of the mechanical arm, but because the variables of each joint of the mechanical arm are mutually coupled, the solution obtained according to an inverse motion equation is not unique, a group of reasonable solutions needs to be selected through repeated calculation for many times according to the combination form of the mechanical arm and the motion range of each joint, and the solution of each joint angle of the mechanical arm through the inverse kinematics is very complex.
In the related art, the joint angles of the mechanical arm are usually determined by two ways, namely a direct calculation way based on a closed solution and a numerical iteration way based on a jacobian matrix. The direct calculation method based on the closed solution has the advantages of high calculation speed, less resource consumption, stable result and the like, but the modeling, resolving and programming processes in the previous stage become very complicated and are not beneficial to development. The modeling process of the numerical iteration mode based on the Jacobian matrix is simple, and the numerical calculation of the Jacobian matrix exists in a common mathematical library, so that the coding is fast, but the calculation speed is slow, and for the mechanical arm with redundant freedom, only minimum norm solutions can be obtained through the pseudo-inverse of the Jacobian matrix, and the solutions are not actually needed (for example, the solved solutions exceed the constraint range of the joint) in many times. Moreover, the numerical iteration mode has the disadvantage of numerical instability near the singular point. Therefore, the joint angles of the mechanical arm cannot be accurately determined by using the related art.
In view of this, the present disclosure provides a method and an apparatus for determining joint angles of a robot arm, a storage medium, and an electronic device, so as to accurately determine the joint angles of the robot arm.
The following detailed description of specific embodiments of the present disclosure is provided in connection with the accompanying drawings. It should be understood that the detailed description and specific examples, while indicating the present disclosure, are given by way of illustration and explanation only, not limitation.
Fig. 1 is a flowchart illustrating a method for determining a joint angle of a robot arm according to an exemplary embodiment. As shown in fig. 1, the method may include the following steps.
In step 101, length information of each joint and current angle information of each joint of the target robot arm are acquired.
It is worth mentioning that, since there is only one solution in the solution space of the inverse kinematics of the robot arm with non-redundant degrees of freedom, it is not necessary to find the optimal solution, and therefore, in the present disclosure, the method for determining the joint angle of the robot arm is mainly applied to the robot arm with redundant degrees of freedom, that is, the method is applied to the robot arm with redundant degrees of freedom. That is, in the disclosure, the target robot arm is a robot arm having redundant degrees of freedom.
The mechanical arm on the ginger robot is composed of joints of 3+1+3 and has six degrees of freedom greater than Euclidean space, namely the mechanical arm on the ginger robot is a mechanical arm with redundant degrees of freedom. Thus, in one embodiment, the target robot arm may be a robot arm of a ginger robot.
In step 102, current position information of the end joint of the target robot arm and a jacobian matrix of the target robot arm are determined according to the length information of each joint and the current angle information of each joint.
It should be noted that, in the present disclosure, the joints of the target robot arm may be referred to as a first joint, a second joint, a third joint, and an … … nth joint in the order from top to bottom. The nth joint is a terminal joint of the target mechanical arm, and the terminal joint can be a joint for grabbing an object. For example, in the present disclosure, the current position information of the target robot arm end joint may be determined by constructing a structural model of the target robot arm. Further, the jacobian matrix of the target robot arm refers to an inverse kinematics jacobian matrix of the robot. The current position information of the distal joint and the manner of determining the jacobian matrix will be described in detail below.
In step 103, a joint angle cost function is constructed according to a preset joint angle optimization condition.
As described above, in the motion process of a mechanical arm with redundant degrees of freedom (e.g., a mechanical arm on a ginger robot), when a ginger wrist (i.e., a terminal joint of the mechanical arm on the ginger robot) has a specific grabbing pose, there are countless sets of solutions of joint angles that can meet the requirement of the ginger wrist for grabbing an object, and therefore, in order to select a set of optimal solutions as a final execution parameter of the mechanical arm, a joint angle optimization condition needs to be preset, a joint angle cost function is constructed according to the joint angle optimization condition, and a set of optimal joint angle solutions is determined based on the joint angle cost function.
In step 104, joint angle increment information is determined using the current position information of the tip joint, the jacobian matrix, and the joint angle cost function.
In step 105, the current angle information of each joint is updated using the joint angle increment information, and the step of determining the current position information of the end joint of the target robot arm is re-executed until the position information of the end joint coincides with the target position.
In the present disclosure, the target position refers to a position to which the target robot arm is finally to reach. For example, when the target robot arm needs to move to position a to grasp an object, position a is the target position.
Further, the joint angle increment information determined in step 104 may be added to the current angle information of each joint acquired in step 101 to achieve updating of the current angle information of each joint. After the current angle information of each joint is updated, the step of determining the current position information of the target robot arm end joint in step 102 may be executed again based on the updated current angle information. When the current position information of the end joint of the target robot arm determined in step 102 does not coincide with the target position, that is, the target robot arm has not yet reached the target position, the step of determining the jacobian matrix of the target robot arm, and subsequent steps 103 to 105, may be continued until the position information of the end joint coincides with the target position.
It should be noted that joint angle incremental information can be accurately determined by using the joint angle cost function, and then current angle information of each joint can be accurately determined once each step 105 is executed, and the determined current angle information of each joint meets a preset joint angle optimization condition, so that angle information of each joint meeting the joint angle optimization condition can be determined in the process that the tail end joint of the target mechanical arm moves from an initial position to a target position, and the rotation angle of the motor is controlled according to the determined angle information of each joint each time, so that the motion of the target mechanical arm can meet the joint angle optimization condition.
By adopting the technical scheme, the joint angle cost function is constructed according to the preset joint angle optimization condition, and then the current position information, the Jacobian matrix and the joint angle cost function of the tail end joint are comprehensively considered when the joint angle of the mechanical arm is determined, so that the motion angle of each joint can be ensured to meet the preset joint angle optimization condition in the process that the target mechanical arm moves to the target position, namely, each joint angle of the mechanical arm can be accurately determined from the infinite solution of the joint angle through the joint angle cost function, and the target mechanical arm can be ensured to move to the target position by adopting the most appropriate path.
In order to make the determination method of the joint angle of the mechanical arm provided by the present disclosure better understood by those skilled in the art, a specific implementation of the method is described below as a complete example.
First, a description will be given of a method of determining current position information of the end joint of the target arm in step 102.
For example, the current position information of the target robot arm end joint may be determined by:
and (1) aiming at each joint of the target mechanical arm, constructing a coordinate system of the joint under a parent coordinate system of the joint according to the length information of the joint and the current angle information of the joint.
Suppose that the target mechanical arm includes three joints, which are respectively marked as a first joint, a second joint and a third joint, and the three joints have lengths of 3, 2 and 2 respectively, and the current angles are respectively a0、a1、a2Then the first joint is in its parent coordinate system0The coordinate system T of the second joint in its parent coordinate system, as shown in equation (1)1As shown in equation (2), the third joint is at its centerCoordinate system T under father coordinate system2As shown in formula (3), the parent coordinate system of the first joint is a world coordinate system, the parent coordinate system of the second joint is a coordinate system of the first joint, and the parent coordinate system of the third joint is a coordinate system of the second joint:
and (2) determining a coordinate system of the tail end joint of the target mechanical arm in a world coordinate system according to the coordinate system of each joint in the parent coordinate system.
According to the step (1), a coordinate system of each joint of the target mechanical arm under the parent coordinate system of the target mechanical arm can be determined respectively, and then the determined coordinate systems of each joint under the parent coordinate system of the target mechanical arm are multiplied in sequence, so that a coordinate system of the tail end joint of the target mechanical arm under the world coordinate system can be obtained.
For example, assuming that the target robot arm includes three joints, and the coordinate systems of the three joints in its parent coordinate system are respectively shown in the above formula (1), formula (2), and formula (3), the coordinate system T of the target robot arm end joint in the world coordinate system can be obtained by formula (4):
T＝T0*T1*T2 (4)
and (3) determining the current position information of the end joint according to the coordinate system of the end joint in the world coordinate system and the length information of the end joint.
Following the above example, the distal joint is a third joint having a length of 2, and the length of the third joint can be expressed as [2 ] in its coordinate system2，0]TConverted to homogeneous coordinates of [2, 0, 1 ]]TThen, the current position information p of the tip joint can be determined by the following formula (5):
thus, the current position information of the end joint can be determined according to the steps (1) to (3).
Next, a method of determining the jacobian matrix of the target robot arm in step 102 will be described.
For example, the jacobian matrix of the target robotic arm may be determined by:
and (4) constructing a current joint angle matrix according to the current angle information of each joint.
And (5) determining partial differential of the current position information of the tail end joint to the current joint angle matrix as a Jacobian matrix of the target mechanical arm.
Illustratively, following the above example, assuming that the target robot arm includes three joints, and the current angles of the three joints are a0, a1, and a2, respectively, the constructed current joint angle matrix may be represented as q ═ a0,a1,a2]T。
Then, partial differentiation of the current position information of the end joint to the current joint angle matrix is performed
Determined as the Jacobian matrix J of the target robot arm, i.e., the Jacobian matrix of the target robot arm
Next, a specific embodiment of constructing the joint angle cost function according to the preset joint angle optimization condition in step 103 will be described.
In practical application, if the joint of the target mechanical arm frequently exceeds the limit, the service life of the joint can be greatly shortened, and the motion of the target mechanical arm is further influenced. Correspondingly, according to the preset joint angle optimization condition, constructing a joint angle cost function may be: and constructing a joint angle cost function according to the limit center of the target joint and the current angle information of the target joint.
It is worth mentioning that the target joint is at least one of the target robot arm joints.
In one possible approach, the target joint is one of the joints in the target robot arm, for example, the first joint in the above example, and the limit center of the first joint is w0Then, the constructed joint angle Cost function can be expressed as Cost ═ (a)0-w0)2Wherein w is0The value of (b) can be 0, 0.5, etc.
In another possible approach, the target joints are two joints in the target robot arm, for example, the second joint and the third joint in the above example, and the limit center of the second joint is w1The limit center of the third joint is w2Then, the constructed joint angle Cost function can be expressed as Cost ═ (a)1-w1)2+(a2-w2)2Wherein w is1And w2May be the same or different.
In yet another possible approach, the target joints are all joints in the target robot arm, i.e., all joint angles in the target robot arm are optimized. Assuming that the target mechanical arm is composed of the first joint, the second joint and the third joint, the constructed joint angle Cost function can be expressed as Cost ═ q-q0)2Wherein q ═ a0,a1,a2]T，q0＝[w0,w1,w2]T。
Next, a specific embodiment of determining the joint angle increment information using the current position information of the distal joint, the jacobian matrix, and the joint angle cost function in step 104 will be described.
Illustratively, as shown in fig. 2, the step 104 may further include steps 1041 to 1043.
In step 1041, position increment information of the end joint is determined based on the current position information of the end joint and the position information of the end joint determined last time.
For example, the difference between the current position information of the end joint and the position information of the end joint determined last time is determined as the position increment information of the end joint.
In step 1042, an optimized gradient direction of the joint angle is determined according to the joint angle cost function.
In the disclosure, the derivation can be performed on the joint angle cost function to obtain the optimized gradient direction of the joint angle. For example, assume that the joint angle Cost function is Cost ═ q-q0)2Then the optimized gradient direction of the joint angle is η ═ 2 (q-q)0)。
It should be noted that, in the present disclosure, the execution sequence of the above steps 1041 and 1042 may not be specifically limited. For example, step 1041 may be performed first and then step 1042 may be performed (as shown in fig. 2), step 1042 may be performed first and then step 1041 may be performed, or step 1041 and step 1042 may be performed simultaneously.
In step 1043, joint angle increment information is determined based on the position increment information, the optimized gradient direction, and the jacobian matrix.
Illustratively, the joint angle increment information may be determined by the following equation (6):
δq＝J+δp+(I-J+J)η (6)
wherein, delta q represents joint angle increment information, J represents Jacobian matrix and J+A pseudo-inverse matrix characterizing the Jacobian matrix, I characterizing the identity matrix of the Jacobian matrix, eta characterizing the optimal gradient direction, and deltap characterizing the distal jointPosition increment information.
Therefore, joint angle increment information can be determined according to the mode, and due to the fact that the optimized gradient direction of the joint angle cost function is considered when the joint angle increment information is determined, the determined joint angle increment information can avoid joint over-limit, joint corners can be guaranteed to be far away from upper and lower limit values as far as possible, and service life of the joint is prolonged.
After the joint angle increment information is determined according to the above mode, the joint angle increment information is superposed on the current angle information of each joint to obtain updated current angle information q of each joint, then the updated current position information p of the tail end joint of the target mechanical arm, the Jacobian matrix J of the target mechanical arm, the joint angle Cost function Cost and the joint angle increment information delta q are sequentially determined according to the steps shown in FIG. 1 based on the updated current angle information q of each joint, the updated current angle information q of each joint is obtained by superposing the joint angle increment information on the current angle information of each joint, and the like, so that the angle information of each joint in the process that the tail end joint of the target mechanical arm moves from the initial position to the target position can be determined, and the motor can be controlled to rotate, so that each joint of the target mechanical arm rotates according to the determined angle information.
Based on the same inventive concept, the invention also provides a device for determining the joint angle of the mechanical arm. Fig. 3 is a block diagram illustrating an apparatus for determining a joint angle of a robot arm according to an exemplary embodiment. As shown in fig. 3, the robot arm joint angle determining apparatus 300 may include:
an obtaining module 301, configured to obtain length information of each joint and current angle information of each joint of the target mechanical arm;
a first determining module 302, configured to determine, according to the length information of each joint and the current angle information of each joint, current position information of a joint at the end of the target robot arm and a jacobian matrix of the target robot arm;
the construction module 303 is configured to construct a joint angle cost function according to a preset joint angle optimization condition;
a second determining module 304, configured to determine joint angle increment information by using the current position information of the end joint, the jacobian matrix, and the joint angle cost function;
and an executing module 305, configured to update the current angle information of each joint by using the joint angle increment information, and re-execute the step of determining the current position information of the end joint of the target robot arm until the position information of the end joint is consistent with the target position.
Optionally, the second determining module 304 may include
The first determining submodule is used for determining position increment information of the tail end joint according to the current position information of the tail end joint and the position information of the tail end joint determined last time;
the second determining submodule is used for determining the optimized gradient direction of the joint angle according to the joint angle cost function;
and the third determining submodule is used for determining joint angle increment information according to the position increment information, the optimized gradient direction and the Jacobian matrix.
Optionally, the third determining sub-module is configured to: determining joint angle increment information by the following formula:
δq＝J+δp+(I-J+J)η
wherein δ q represents the joint angle increment information, J represents the Jacobian matrix, J+And characterizing a pseudo-inverse matrix of the Jacobian matrix, I characterizing a unit matrix of the Jacobian matrix, eta characterizing the optimized gradient direction, and δ p characterizing position increment information of the tail end joint.
Optionally, the joint angle optimization condition is that the angle of the target joint is close to the limit center of the target joint;
the building module 303 is configured to: and constructing a joint angle cost function according to the limit center of the target joint and the current angle information of the target joint.
Optionally, the target joint is at least one of the target robot arm joints.
Optionally, the first determining module 302 includes:
the first construction submodule is used for constructing a coordinate system of each joint of the target mechanical arm under a parent coordinate system of the joint according to the length information of the joint and the current angle information of the joint;
the fourth determining submodule is used for determining a coordinate system of the tail end joint of the target mechanical arm in a world coordinate system according to the coordinate system of each joint in the parent coordinate system of each joint;
and the fifth determining submodule is used for determining the current position information of the terminal joint according to the coordinate system of the terminal joint in the world coordinate system and the length information of the terminal joint.
Optionally, the first determining module 302 includes:
the second construction submodule is used for constructing a current joint angle matrix according to the current angle information of each joint;
and the sixth determining submodule is used for determining the partial differential of the position information of the tail end joint to the current joint angle matrix as a Jacobian matrix of the target mechanical arm.
With regard to the apparatus in the above-described embodiment, the specific manner in which each module performs the operation has been described in detail in the embodiment related to the method, and will not be elaborated here.
Based on the same inventive concept, the present disclosure also provides a robot, which includes a mechanical arm, and the joint angle of the mechanical arm is determined by the determination method of the joint angle of the mechanical arm provided by the present disclosure; a memory having a computer program stored thereon; a processor for executing the computer program in the memory to implement the steps of the method for determining a joint angle of a robotic arm.
Illustratively, FIG. 4 is a block diagram illustrating a robot in accordance with an exemplary embodiment. As shown in fig. 4, the robot 700 may include: a processor 701 and a memory 702. The electronic device 700 may also include one or more of a multimedia component 703, an input/output (I/O) interface 704, and a communication component 705.
The processor 701 is configured to control the overall operation of the electronic device 700 to complete all or part of the steps in the method for determining the joint angle of the robot arm. The memory 702 is used to store various types of data to support operation at the electronic device 700, such as instructions for any application or method operating on the electronic device 700 and application-related data, such as contact data, transmitted and received messages, pictures, audio, video, and the like. The Memory 702 may be implemented by any type of volatile or non-volatile Memory device or combination thereof, such as Static Random Access Memory (SRAM), Electrically Erasable Programmable Read-Only Memory (EEPROM), Erasable Programmable Read-Only Memory (EPROM), Programmable Read-Only Memory (PROM), Read-Only Memory (ROM), magnetic Memory, flash Memory, magnetic disk, or optical disk. The multimedia components 703 may include screen and audio components. Wherein the screen may be, for example, a touch screen and the audio component is used for outputting and/or inputting audio signals. For example, the audio component may include a microphone for receiving external audio signals. The received audio signal may further be stored in the memory 702 or transmitted through the communication component 705. The audio assembly also includes at least one speaker for outputting audio signals. The I/O interface 704 provides an interface between the processor 701 and other interface modules, such as a keyboard, mouse, buttons, etc. These buttons may be virtual buttons or physical buttons. The communication component 705 is used for wired or wireless communication between the electronic device 700 and other devices. Wireless Communication, such as Wi-Fi, bluetooth, Near Field Communication (NFC), 2G, 3G, 4G, NB-IOT, eMTC, or other 5G, etc., or a combination of one or more of them, which is not limited herein. The corresponding communication component 705 may thus include: Wi-Fi module, Bluetooth module, NFC module, etc.
In an exemplary embodiment, the electronic Device 700 may be implemented by one or more Application Specific Integrated Circuits (ASICs), Digital Signal Processors (DSPs), Digital Signal Processing Devices (DSPDs), Programmable Logic Devices (PLDs), Field Programmable Gate Arrays (FPGAs), controllers, microcontrollers, microprocessors, or other electronic components for performing the above-described method for determining the joint angle of the robot arm.
In another exemplary embodiment, a computer readable storage medium comprising program instructions which, when executed by a processor, implement the steps of the method for determining a joint angle of a robot arm as described above is also provided. For example, the computer readable storage medium may be the memory 702 described above comprising program instructions executable by the processor 701 of the electronic device 700 to perform the method for determining the joint angle of a robotic arm described above.
In another exemplary embodiment, a computer program product is also provided, which comprises a computer program executable by a programmable apparatus, the computer program having code portions for performing the above-described method of determining a joint angle of a robot arm when executed by the programmable apparatus.
The preferred embodiments of the present disclosure are described in detail with reference to the accompanying drawings, however, the present disclosure is not limited to the specific details of the above embodiments, and various simple modifications may be made to the technical solution of the present disclosure within the technical idea of the present disclosure, and these simple modifications all belong to the protection scope of the present disclosure.
It should be noted that the various features described in the above embodiments may be combined in any suitable manner without departing from the scope of the invention. In order to avoid unnecessary repetition, various possible combinations will not be separately described in this disclosure.
In addition, any combination of various embodiments of the present disclosure may be made, and the same should be considered as the disclosure of the present disclosure, as long as it does not depart from the spirit of the present disclosure.
Claims (10)
1. A method for determining a joint angle of a mechanical arm is characterized by comprising the following steps:
acquiring length information of each joint of a target mechanical arm and current angle information of each joint;
determining the current position information of the tail end joint of the target mechanical arm and a Jacobian matrix of the target mechanical arm according to the length information of each joint and the current angle information of each joint;
constructing a joint angle cost function according to a preset joint angle optimization condition;
determining joint angle increment information by using the current position information of the tail end joint, the Jacobian matrix and the joint angle cost function;
and updating the current angle information of each joint by using the joint angle increment information, and re-executing the step of determining the current position information of the tail end joint of the target mechanical arm until the position information of the tail end joint is consistent with the target position.
2. The method of determining according to claim 1, wherein said determining joint angle increment information using the current position information of the tip joint, the jacobian matrix, and the joint angle cost function comprises:
determining position increment information of the tail end joint according to the current position information of the tail end joint and the position information of the tail end joint determined last time;
determining an optimized gradient direction of the joint angle according to the joint angle cost function;
and determining joint angle increment information according to the position increment information, the optimized gradient direction and the Jacobian matrix.
3. The method of claim 2, wherein determining joint angle increment information based on the position increment information, the optimized gradient direction, and the jacobian matrix comprises:
determining joint angle increment information by the following formula:
δq＝J+δp+(I-J+J)η
wherein δ q represents the joint angle increment information, J represents the Jacobian matrix, J+And characterizing a pseudo-inverse matrix of the Jacobian matrix, I characterizing a unit matrix of the Jacobian matrix, eta characterizing the optimized gradient direction, and δ p characterizing position increment information of the tail end joint.
4. The determination method according to any one of claims 1 to 3, wherein the joint angle optimization condition is that an angle of a target joint is close to a limit center of the target joint;
the constructing of the joint angle cost function according to the preset joint angle optimization condition comprises the following steps:
and constructing a joint angle cost function according to the limit center of the target joint and the current angle information of the target joint.
5. The determination method according to claim 4, wherein the target joint is at least one of the target robot arm joints.
6. A determination method according to any one of claims 1-3, characterized in that the current position information of the target robot arm tip joint is determined by:
aiming at each joint of the target mechanical arm, constructing a coordinate system of the joint under a parent coordinate system of the joint according to the length information of the joint and the current angle information of the joint;
determining a coordinate system of the tail end joint of the target mechanical arm in a world coordinate system according to the coordinate system of each joint in the parent coordinate system of each joint;
and determining the current position information of the end joint according to the coordinate system of the end joint in the world coordinate system and the length information of the end joint.
7. A determination method according to any one of claims 1-3, characterized in that the jacobian matrix of the target robot arm is determined by:
constructing a current joint angle matrix according to the current angle information of each joint;
and determining the partial differential of the current position information of the tail end joint to the current joint angle matrix as the Jacobian matrix of the target mechanical arm.
8. An apparatus for determining a joint angle of a robot arm, comprising:
the acquisition module is used for acquiring length information of each joint of the target mechanical arm and current angle information of each joint;
the first determining module is used for determining the current position information of the tail end joint of the target mechanical arm and the Jacobian matrix of the target mechanical arm according to the length information of each joint and the current angle information of each joint;
the construction module is used for constructing a joint angle cost function according to a preset joint angle optimization condition;
the second determination module is used for determining joint angle increment information by utilizing the current position information of the tail end joint, the Jacobian matrix and the joint angle cost function;
and the execution module is used for updating the current angle information of each joint by using the joint angle increment information and re-executing the step of determining the current position information of the tail end joint of the target mechanical arm until the position information of the tail end joint is consistent with the target position.
9. A computer-readable storage medium, on which a computer program is stored which, when being executed by a processor, carries out the steps of the method according to any one of claims 1 to 7.
10. A robot, comprising:
a robotic arm, and the joint angle of the robotic arm is determined by the method of any one of claims 1-7;
a memory having a computer program stored thereon;
a processor for executing the computer program in the memory to carry out the steps of the method of any one of claims 1 to 7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110706562.8A CN113442137A (en) | 2021-06-24 | 2021-06-24 | Method and device for determining joint angle of mechanical arm, storage medium and electronic equipment |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110706562.8A CN113442137A (en) | 2021-06-24 | 2021-06-24 | Method and device for determining joint angle of mechanical arm, storage medium and electronic equipment |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN113442137A true CN113442137A (en) | 2021-09-28 |
Family
ID=77812467
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110706562.8A Pending CN113442137A (en) | 2021-06-24 | 2021-06-24 | Method and device for determining joint angle of mechanical arm, storage medium and electronic equipment |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113442137A (en) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109648564A (en) * | 2019-01-15 | 2019-04-19 | 西安科技大学 | A kind of control method of the multiple degrees of freedom flexible articulated mechanical arm system based on hierarchical structure MPC |
| CN110434854A (en) * | 2019-08-20 | 2019-11-12 | 兰州大学 | A kind of redundancy mechanical arm Visual servoing control method and apparatus based on data-driven |
| CN111890363A (en) * | 2020-07-27 | 2020-11-06 | 四川大学 | Mechanical arm motion planning method based on rapid self-adaptive gradient neural network algorithm |
| CN112091979A (en) * | 2020-10-10 | 2020-12-18 | 哈尔滨工业大学 | Seven-degree-of-freedom mechanical arm limiting optimization method based on position-level inverse kinematics |
| CN112597437A (en) * | 2020-12-29 | 2021-04-02 | 深圳市优必选科技股份有限公司 | Method, device and equipment for analyzing inverse kinematics of mechanical arm |
-
2021
- 2021-06-24 CN CN202110706562.8A patent/CN113442137A/en active Pending
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109648564A (en) * | 2019-01-15 | 2019-04-19 | 西安科技大学 | A kind of control method of the multiple degrees of freedom flexible articulated mechanical arm system based on hierarchical structure MPC |
| CN110434854A (en) * | 2019-08-20 | 2019-11-12 | 兰州大学 | A kind of redundancy mechanical arm Visual servoing control method and apparatus based on data-driven |
| CN111890363A (en) * | 2020-07-27 | 2020-11-06 | 四川大学 | Mechanical arm motion planning method based on rapid self-adaptive gradient neural network algorithm |
| CN112091979A (en) * | 2020-10-10 | 2020-12-18 | 哈尔滨工业大学 | Seven-degree-of-freedom mechanical arm limiting optimization method based on position-level inverse kinematics |
| CN112597437A (en) * | 2020-12-29 | 2021-04-02 | 深圳市优必选科技股份有限公司 | Method, device and equipment for analyzing inverse kinematics of mechanical arm |
Non-Patent Citations (4)
| Title |
|---|
| 曹昱: "七轴冗余机械臂容错控制研究", 《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑》 * |
| 李鹏浩: "七自由度冗余空间机械臂多目标轨迹规划研究", 《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑》 * |
| 潘博等: "面向冗余机器人实时控制的逆运动学求解有效方法", 《控制与决策》 * |
| 贾庆轩等: "考虑关节限位的冗余度机器人重复性路径规划研究", 《高技术通讯》 * |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| WO2018086226A1 (en) | Control method and apparatus for robotic arm | |
| Rossmann et al. | A new approach to 3d simulation technology as enabling technology for erobotics | |
| Ma et al. | Robust uncalibrated visual servoing control based on disturbance observer | |
| Tan et al. | Robust model-free control for redundant robotic manipulators based on zeroing neural networks activated by nonlinear functions | |
| Chen et al. | Super-twisting ZNN for coordinated motion control of multiple robot manipulators with external disturbances suppression | |
| CN115401699B (en) | Industrial robot precision reliability analysis method, device, equipment and storage medium | |
| CN113119104B (en) | Mechanical arm control method, mechanical arm control device, computing equipment and system | |
| CN113119081A (en) | Inverse solution method and device for arm angle interval of mechanical arm and terminal equipment | |
| Zhang et al. | Voice control dual arm robot based on ROS system | |
| CN111487967A (en) | Robot path planning method, device, medium and equipment | |
| Rea Minango et al. | Combining the STEP-NC standard and forward and inverse kinematics methods for generating manufacturing tool paths for serial and hybrid robots | |
| CN112045664A (en) | General mechanical arm controller based on ROS system | |
| US20200147794A1 (en) | Techniques for cad-informed robotic assembly | |
| CN117359639A (en) | Robot cluster cooperative control method and system | |
| CN113442137A (en) | Method and device for determining joint angle of mechanical arm, storage medium and electronic equipment | |
| CN113084791B (en) | Mechanical arm control method, mechanical arm control device and terminal equipment | |
| CN113124872A (en) | Robot positioning navigation method and device, terminal equipment and robot | |
| CN112356032A (en) | Posture smooth transition method and system | |
| CN115816459A (en) | Robot control method, device, computer equipment, storage medium and product | |
| Kong et al. | A time-specified zeroing neural network for quadratic programming with its redundant manipulator application | |
| CN113199481B (en) | Robot motion control method, device, electronic apparatus, and medium | |
| Lazarević | Iterative learning control of integer and noninteger order: An overview | |
| Kong et al. | A repeatable optimization for kinematic energy system with its mobile manipulator application | |
| Liu et al. | Evolving hidden Markov model based human intention learning and inference | |
| Ba | A flexible sliding mode controller for robot manipulators using a new type of neural-network predictor |
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 | ||
| CB02 | Change of applicant information |
Address after: 201111 Building 8, No. 207, Zhongqing Road, Minhang District, Shanghai Applicant after: Dayu robot Co.,Ltd. Address before: 201111 2nd floor, building 2, no.1508, Kunyang Road, Minhang District, Shanghai Applicant before: Dalu Robot Co.,Ltd. |
|
| CB02 | Change of applicant information | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20210928 |
|
| RJ01 | Rejection of invention patent application after publication |