Joint pose determination method, device and equipment of mechanical arm and medium

Abstract

This invention discloses a method, apparatus, device, and medium for determining the joint pose of a robotic arm. The method includes: acquiring the current pose matrix of a target redundant joint in a target robot and a user-triggered target robotic arm control command; determining a reference joint position of the target execution joint based on the current pose matrix, the target robotic arm control command, and a preset inverse kinematics equation corresponding to the target robotic arm; determining a reference operating current of the target redundant joint based on the current pose matrix, the reference joint position, and a preset robotic arm dynamics model corresponding to the target robotic arm; and accurately and conveniently determining the target pose matrix of the target redundant joint based on the current difference between the current and the reference operating current, thereby enabling timely adjustment of the target redundant joint using the determined target pose matrix when a collision occurs, and effectively mitigating the collision.

Description

TECHNICAL FIELD

[0001] The embodiment of the application relates to the technical field of medical equipment, and particularly relates to a joint pose determination method and device of a mechanical arm, equipment and a medium. BACKGROUND

[0002] With the development of technology, surgical robots have been gradually applied to modern medicine and have become one of the important means to improve surgical precision and reduce surgical risks. In actual operation, when a doctor controls the mechanical arm of a surgical robot, the doctor needs to avoid the mechanical arm from colliding with the surrounding environment or the doctor's own mechanical arm, so as to ensure the normal operation of the mechanical arm and the normal progress of the surgery.

[0003] At present, a sensor detection method is usually used to detect the collision of the mechanical arm of the surgical robot to avoid the collision of the mechanical arm. However, in order to effectively avoid the collision of the mechanical arm, the sensor detection method needs to use a high-precision sensor, which greatly increases the cost of collision detection. When the sensor fails or the installation position deviates, the collision of the mechanical arm cannot be effectively avoided in time, and the mechanical arm will be powered off or manually reset after power failure to protect the mechanical arm, so that the mechanical arm cannot be adjusted in time after the collision to ensure the continuity of the surgery. SUMMARY

[0004] The embodiment of the application provides a joint pose determination method, device, equipment and medium of a mechanical arm, so that the target pose matrix of a target redundant joint can be accurately and conveniently determined, the target redundant joint is adjusted in time by using the determined target pose matrix when the mechanical arm collides, and the collision can be effectively eliminated when the mechanical arm collides.

[0005] In a first aspect, the embodiment of the application provides a joint pose determination method of a mechanical arm, comprising:

[0006] obtaining a current pose matrix of a target redundant joint in a target robot and a target mechanical arm control instruction triggered by a user; the target mechanical arm of the target robot comprises a target redundant joint and a target execution joint;

[0007] determining a reference execution joint position of the target execution joint based on the current pose matrix, the target mechanical arm control instruction and a preset inverse kinematics equation corresponding to the target mechanical arm;

[0008] determining a reference working current of the target redundant joint based on the current pose matrix, the reference execution joint position and a preset mechanical arm dynamics model corresponding to the target mechanical arm;

[0009] The target pose matrix of the target redundant joint is determined based on the current difference between the current operating current and the reference operating current; the current operating current is the current generated by the target redundant joint when the target execution joint moves to the position of the reference execution joint.

[0010] Optionally, the method further includes: obtaining the current redundant joint position of the target redundant joint in the target robot; determining the current pose matrix of the target redundant joint based on the current redundant joint position and the preset pose matrix calculation formula corresponding to the target redundant joint; wherein the preset pose matrix calculation formula is a pose matrix calculation formula that rotates around a preset direction in the target coordinate system.

[0011] Optionally, the method further includes: determining a reference motion command for the target execution joint based on the current pose matrix and the target robotic arm control command; and determining a reference execution joint position for the target execution joint based on the reference motion command and the preset inverse kinematics equation corresponding to the target robotic arm.

[0012] Optionally, the method further includes: obtaining the current redundant joint position of the target redundant joint by extracting the position from the current pose matrix; determining the reference operating current of the target redundant joint based on the current redundant joint position, the reference execution joint position, the preset motor torque constant corresponding to the target manipulator, and the preset manipulator dynamics model corresponding to the target manipulator; wherein the preset manipulator dynamics model is constructed based on the preset manipulator dynamics equation corresponding to the target manipulator.

[0013] Optionally, the method further includes: obtaining the current redundant joint velocity and current redundant joint acceleration of the target redundant joint by differentiating the current redundant joint position; obtaining the reference execution joint velocity and reference execution joint acceleration of the target execution joint by differentiating the reference execution joint position; inputting the current redundant joint position, the current redundant joint velocity, the current redundant joint acceleration, the reference execution joint position, the reference execution joint velocity, the reference execution joint acceleration, and the preset motor torque constant corresponding to the target robotic arm into the preset robotic arm dynamics model corresponding to the target robotic arm, and determining the reference operating current of the target redundant joint based on the output of the preset robotic arm dynamics model.

[0014] Optionally, the method further includes: determining the current operating state of the target robotic arm based on the current difference between the current operating current and the reference operating current; determining the target redundant joint acceleration of the target redundant joint based on the current operating state, the current difference, the target inherent parameters of the target redundant joint, the preset motor torque constant corresponding to the target robotic arm, the current redundant joint position, and the current redundant joint speed; the target inherent parameters include: the virtual mass, damping, and stiffness coefficient of the target redundant joint; and obtaining the target pose matrix of the target redundant joint by integrating the target redundant joint acceleration.

[0015] Optionally, the method further includes: multiplying the current difference by a preset motor torque constant corresponding to the target robotic arm to obtain a first multiplication result; multiplying the damping of the target redundant joint by the current redundant joint velocity to obtain a second multiplication result; multiplying the stiffness coefficient of the target redundant joint by the current redundant joint position to obtain a third multiplication result; summing the second multiplication result and the third multiplication result to obtain an addition result; subtracting the first multiplication result from the addition result to obtain a subtraction result; and determining the ratio of the subtraction result to the virtual mass of the target redundant joint as the target redundant joint acceleration.

[0016] Secondly, embodiments of the present invention also provide a joint pose determination device for a robotic arm, the device comprising:

[0017] The information acquisition module is used to acquire the current pose matrix of the target redundant joints in the target robot and the target robotic arm control command triggered by the user; the target robotic arm of the target robot includes: target redundant joints and target execution joints;

[0018] The reference joint position determination module is used to determine the reference joint position of the target execution joint based on the current pose matrix, the target manipulator control command, and the preset inverse kinematics equation corresponding to the target manipulator.

[0019] The reference operating current determination module is used to determine the reference operating current of the target redundant joint based on the current pose matrix, the position of the reference execution joint, and the preset manipulator dynamics model corresponding to the target manipulator.

[0020] The target pose matrix determination module is used to determine the target pose matrix of the target redundant joint based on the current difference between the current operating current and the reference operating current; the current operating current is the current generated by the target redundant joint when the target execution joint moves to the position of the reference execution joint.

[0021] Thirdly, embodiments of the present invention also provide an electronic device, the electronic device comprising:

[0022] One or more processors;

[0023] Memory, used to store one or more programs;

[0024] When the one or more programs are executed by the one or more processors, the one or more processors implement the joint pose determination method for a robotic arm as provided in any embodiment of the present invention.

[0025] Fourthly, embodiments of the present invention provide a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the joint pose determination method for a robotic arm as provided in any embodiment of the present invention.

[0026] Fifthly, embodiments of the present invention provide a computer program product, including a computer program that, when executed by a processor, implements the joint pose determination method for a robotic arm as provided in any embodiment of the present invention.

[0027] The technical solution of this invention involves acquiring the current pose matrix of a target redundant joint in a target robot and a user-triggered target robotic arm control command. The target robotic arm of the target robot includes a target redundant joint and a target execution joint. Based on the current pose matrix, the target robotic arm control command, and the preset inverse kinematics equation corresponding to the target robotic arm, a reference execution joint position of the target execution joint is determined. When the target robot controls the target execution joint to move to the reference execution joint position, the target redundant joint generates a current operating current. Based on the current pose matrix, the reference execution joint position, and the preset robotic arm dynamics model corresponding to the target robotic arm, a reference operating current of the target redundant joint is determined. Based on the current difference between the current operating current and the reference operating current, the target pose matrix of the target redundant joint can be accurately and conveniently determined. This allows for timely adjustment of the target redundant joint using the determined target pose matrix when the robotic arm collides, thereby effectively mitigating the collision.

[0028] It should be understood that the description in this section is not intended to identify key or essential features of the embodiments of the present invention, nor is it intended to limit the scope of the invention. Other features of the invention will become readily apparent from the following description. Attached Figure Description

[0029] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0030] Figure 1 This is a flowchart of a method for determining the joint pose of a robotic arm according to Embodiment 1 of the present invention;

[0031] Figure 2 This is a flowchart of a method for determining the joint pose of a robotic arm according to Embodiment 2 of the present invention;

[0032] Figure 3 This is a schematic diagram of the joint pose determination device for a robotic arm provided in Embodiment 3 of the present invention;

[0033] Figure 4 This is a schematic diagram of the structure of an electronic device that implements the joint pose determination method of the robotic arm according to an embodiment of the present invention. Detailed Implementation

[0034] To enable those skilled in the art to better understand the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0035] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that the embodiments of the invention described herein can be implemented in orders other than those illustrated or described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover a non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0036] Example 1

[0037] Figure 1This is a flowchart of a method for determining the joint pose of a robotic arm according to Embodiment 1 of the present invention. This embodiment is applicable to the case of determining the target pose matrix of a target redundant joint. This method can be executed by a joint pose determination device for the robotic arm, which can be implemented in hardware and / or software and can be configured in an electronic device. Figure 1 As shown, the method includes:

[0038] S110. Obtain the current pose matrix of the redundant joints in the target robot and the target robotic arm control command triggered by the user.

[0039] The target robotic arm of the target robot includes a target redundant joint and a target execution joint. The target robot can refer to a surgical robot undergoing periodic robotic arm collision detection. For example, the target robot can be, but is not limited to, a laparoscopic surgical robot. The target robotic arm can refer to the robotic arm assembled on the target robot. The target robotic arm can be used to hold instruments and perform surgical operations. In this embodiment, the target robotic arm needs to have at least one additional joint for adjusting the shape and position of the robotic arm, in addition to the necessary joints required to complete the surgical operation. The target redundant joint can refer to a joint used to adjust the shape and position of the target robotic arm. The target execution joint can refer to a joint used to perform surgical operations in response to robotic arm control commands. For example, if the target robotic arm has seven joints, the first joint at the root is the target redundant joint, and the remaining six joints are target execution joints. The target redundant joint can be connected to the target robot base. The last target execution joint, such as the seventh joint, is connected to the robotic arm. The current pose matrix can refer to the pose matrix of the target redundant joint at the current moment when the target robotic arm executes the target robotic arm control commands. The current pose matrix can be a 4×4 matrix T. r , represented as Where R represents a 3×3 rotation matrix and t represents a 3×1 position matrix. If the target robotic arm does not collide with anything, the current pose matrix remains unchanged. The target robotic arm control command can be the control command issued by the user to control the target robot's operation. In this embodiment, the control command can be the command matrix T obtained by converting the user-triggered control command. d .

[0040] Specifically, the current pose matrix of the target redundant joints in the target robot is obtained. The user performs surgical operations on the operating terminal, so that the operating terminal can translate the user's surgical operations into target robotic arm control commands that the target robot can understand, and send the target robotic arm control commands to the target robot.

[0041] Based on the above technical solution, "obtaining the current pose matrix of the target redundant joint in the target robot" may include: obtaining the current position of the target redundant joint in the target robot; determining the current pose matrix of the target redundant joint based on the current position of the redundant joint and the preset pose matrix calculation formula corresponding to the target redundant joint; wherein, the preset pose matrix calculation formula is the pose matrix calculation formula of rotating around a preset direction in the target coordinate system.

[0042] Here, the current redundant joint position refers to the position of the target redundant joint at the current moment when the target robotic arm executes the target robotic arm control command. The current redundant joint position can be represented as θ. r The preset pose matrix calculation formula can refer to a pre-set formula for calculating the pose matrix based on position. Alternatively, it can be a formula for calculating the pose matrix after rotation around the z-axis in the target's 3D coordinate system. The preset pose matrix calculation formula can be represented as Rotz().

[0043] Specifically, the current redundant joint position θ of the target redundant joint in the target robot is obtained. r and set the current redundant joint position θ r Substitute the values ​​into the preset pose matrix calculation formula Rotz() corresponding to the target redundant joint to determine the current pose matrix Rotz(θ) of the target redundant joint. r ).in,

[0044] S120. Based on the current pose matrix, the target robotic arm control command, and the preset inverse kinematics equation corresponding to the target robotic arm, determine the reference joint position of the target execution joint.

[0045] The preset inverse kinematics equations can be constructed in advance based on the target robotic arm's attribute information and inverse kinematics equations. The preset inverse kinematics equations can be expressed as F... inv The attribute information may include, but is not limited to, the number of joints and degrees of freedom of the target robotic arm. The reference execution joint position refers to the position that the target execution joint needs to move to when executing target robotic arm control commands. When executing target robotic arm control commands, the target robotic arm can control the operation of the target execution joint according to the reference execution joint position. During movement, it is necessary to detect the current operating current generated by the target redundant joints to promptly determine whether a collision has occurred, and to adjust in a timely manner to effectively resolve the collision if one occurs.

[0046] Specifically, the motion matrix that the target manipulator joint needs to complete when executing the target manipulator control command is derived by using the current pose matrix and the target manipulator control command. The obtained motion matrix corresponding to the target manipulator joint is then substituted into the preset inverse kinematics equation corresponding to the target manipulator to determine the reference joint position of the target manipulator joint.

[0047] Based on the above technical solution, "determining the reference joint position of the target execution joint based on the current pose matrix, the target robotic arm control command, and the preset inverse kinematics equation corresponding to the target robotic arm" can include: determining the reference motion command of the target execution joint based on the current pose matrix and the target robotic arm control command; and determining the reference joint position of the target execution joint based on the reference motion command and the preset inverse kinematics equation corresponding to the target robotic arm.

[0048] The reference motion command refers to the action command that the target robotic arm's joints need to perform when executing the target robotic arm control commands. The current motion execution can be represented in matrix form.

[0049] Specifically, the current pose matrix T r Perform transpose processing to obtain the transposed current pose matrix. The transposed current pose matrix Control command T of the target robotic arm d Perform matrix multiplication ( T d ), obtain the reference motion command T of the target joint. m Refer to motion command T m The preset inverse kinematics equation F is input to the target robotic arm. inv The reference joint position θ of the target joint is obtained. list Among them, the reference joint position θ list =F inv (T m Reference joint position θ list It contains the positions of all target joints and is a (N-1)×1 position matrix, where N is the total number of joints in the target robotic arm. Continuing with the previous example, N is 7.

[0050] S130. Based on the current pose matrix, the reference execution joint position, and the preset manipulator dynamics model corresponding to the target manipulator, determine the reference operating current of the target redundant joint.

[0051] The preset robotic arm dynamics model can be constructed based on the preset robotic arm dynamics equations corresponding to the target robotic arm. The preset robotic arm dynamics equations can be represented as L. These equations can be constructed based on the target robotic arm's attribute information and traditional robotic arm dynamics equations. Attribute information may include, but is not limited to, the number of joints and degrees of freedom of the target robotic arm. The reference operating current refers to the ideal operating current that the target redundant joints should generate when the target robotic arm executes target robotic arm control commands. For example, the reference operating current can be the ideal operating current that the target redundant joints should generate when the target execution joint moves to the reference execution joint position. The ideal operating current can be the operating current under the premise of no collision.

[0052] Specifically, the position in the current pose matrix and the reference joint position are input to the preset manipulator dynamics model corresponding to the target manipulator, and the reference operating current of the target redundant joint is determined based on the output of the preset manipulator dynamics model.

[0053] Based on the above technical solution, "determining the reference operating current of the target redundant joint based on the current pose matrix, the reference execution joint position, and the preset manipulator dynamics model corresponding to the target manipulator" can include: extracting the position of the target redundant joint by performing position extraction on the current pose matrix; determining the reference operating current of the target redundant joint based on the current redundant joint position, the reference execution joint position, the preset motor torque constant corresponding to the target manipulator, and the preset manipulator dynamics model corresponding to the target manipulator; wherein, the preset manipulator dynamics model is constructed based on the preset manipulator dynamics equation corresponding to the target manipulator.

[0054] Specifically, the current redundant joint position of the target redundant joint is obtained by extracting the position from the current pose matrix. If the user inputs a preset motor torque constant corresponding to the target robotic arm, the current redundant joint position, the reference execution joint position, and the preset motor torque constant are input into the preset robotic arm dynamics model, and the reference operating current of the target redundant joint is determined based on the output of the preset robotic arm dynamics model. If the user does not input a preset motor torque constant corresponding to the target robotic arm, the default value of the motor torque constant is assumed, and the current redundant joint position and the reference execution joint position are input into the preset robotic arm dynamics model, and the reference operating current of the target redundant joint is determined based on the output of the preset robotic arm dynamics model. This allows for the determination of a more accurate reference operating current for the target redundant joint using an accurate motor torque constant, further improving the accuracy of the target pose matrix determination.

[0055] Based on the above technical solution, "determining the reference operating current of the target redundant joint based on the current redundant joint position, the reference execution joint position, the preset motor torque constant corresponding to the target manipulator, and the preset manipulator dynamics model corresponding to the target manipulator" can include: obtaining the current redundant joint velocity and current redundant joint acceleration of the target redundant joint by differentiating the current redundant joint position; obtaining the reference execution joint velocity and reference execution joint acceleration of the target execution joint by differentiating the reference execution joint position; inputting the current redundant joint position, current redundant joint velocity, current redundant joint acceleration, reference execution joint position, reference execution joint velocity, reference execution joint acceleration, and the preset motor torque constant corresponding to the target manipulator into the preset manipulator dynamics model corresponding to the target manipulator, and determining the reference operating current of the target redundant joint based on the output of the preset manipulator dynamics model.

[0056] The preset robotic arm dynamics model can be a domain adaptive model. The current redundant joint velocity, current redundant joint acceleration, reference executed joint velocity, reference executed joint acceleration, and current redundant joint torque are all calculated as vectors.

[0057] Specifically, by differentiating the current redundant joint position, the current redundant joint velocity (first derivative) and current redundant joint acceleration (second derivative) of the target redundant joint are obtained. Similarly, by differentiating the reference execution joint position, the reference execution joint velocity (first derivative) and reference execution joint acceleration (second derivative) of the target execution joint are obtained. The current redundant joint position, current redundant joint velocity, current redundant joint acceleration, reference execution joint position, reference execution joint velocity, reference execution joint acceleration, and the preset motor torque constant corresponding to the target robotic arm are then input into the preset robotic arm dynamics model corresponding to the target robotic arm. Based on the output of the preset robotic arm dynamics model, the reference operating current of the target redundant joint is determined. Wherein, Θ, These represent the position, velocity vector, and acceleration vector of all joints, respectively, and K. m τ is the preset motor torque constant. c I is the vector of calculated joint torque values. c This is a vector of calculated joint current values.

[0058] S140. Based on the current difference between the current operating current and the reference operating current, determine the target pose matrix of the target redundant joint.

[0059] Here, the current operating current is the current generated by the target redundant joint when the target actuator joint moves to the reference actuator joint position. The target pose matrix can refer to the safe pose that the target redundant joint needs to be adjusted to in order to avoid a collision with the target robotic arm. The current operating current can be represented as I. ra The reference operating current can be expressed as I. rc .

[0060] Specifically, the current operating current I ra With reference operating current I rc Subtraction (I) ra -I rc The current difference ΔI is obtained. r If the current difference is less than or equal to zero, the current pose matrix is ​​used as the target pose matrix of the target redundant joint to keep the current pose matrix unchanged, and the reference execution joint position is used as the target execution joint position to ensure that the reference execution joint position remains unchanged. This allows the target robotic arm to complete the target robotic arm control commands without collision, further ensuring that the end-effector pose of the target robotic arm remains unchanged, and enabling the target robotic arm to accurately reproduce the target robotic arm control commands.

[0061] The technical solution of this invention involves acquiring the current pose matrix of the target redundant joint in the target robot and the user-triggered target robotic arm control command. The target robotic arm of the target robot includes a target redundant joint and a target execution joint. Based on the current pose matrix, the target robotic arm control command, and the preset inverse kinematics equation corresponding to the target robotic arm, the reference execution joint position of the target execution joint is determined. When the target robot controls the target execution joint to move to the reference execution joint position, the target redundant joint generates a current operating current. Based on the current pose matrix, the reference execution joint position, and the preset robotic arm dynamics model corresponding to the target robotic arm, the reference operating current of the target redundant joint is determined. Based on the current difference between the current and the reference operating current, the target pose matrix of the target redundant joint can be accurately and conveniently determined. This allows for timely adjustment of the target redundant joint using the determined target pose matrix when the robotic arm collides, thereby effectively resolving the collision.

[0062] Example 2

[0063] Figure 2 This is a flowchart of a method for determining the joint pose of a robotic arm according to Embodiment 2 of the present invention. Based on the above embodiments, this embodiment describes in detail the process of determining the target pose matrix of the target redundant joint using current differences. Explanations of terms that are the same as or corresponding to those in the above embodiments are not repeated here. Figure 2 As shown, the method includes:

[0064] S210. Obtain the current pose matrix of the target redundant joints in the target robot and the target robotic arm control command triggered by the user.

[0065] The target robotic arm of the target robot includes: target redundant joints and target execution joints.

[0066] S220. Based on the current pose matrix, the target robotic arm control commands, and the preset inverse kinematics equations corresponding to the target robotic arm, determine the reference joint position of the target robotic joint.

[0067] S230. Based on the current pose matrix, the reference execution joint position, and the preset manipulator dynamics model corresponding to the target manipulator, determine the reference operating current of the target redundant joint.

[0068] S240. Determine the current working state of the target robotic arm based on the current difference between the current working current and the reference working current.

[0069] The current operating current is the current generated by the redundant joints of the target robot when the target joint moves to the position of the reference joint. The current operating state can be used to characterize whether a collision occurs when the target robot arm executes control commands. The current operating state can include: a collision-free state and a collision-occurring state.

[0070] Specifically, if the current difference is less than or equal to zero, the current operating state of the target robotic arm is determined to be a collision-free state. If the current difference is greater than zero, it indicates that the target robotic arm has just experienced a collision, and the current operating state of the target robotic arm is determined to be a collision-occurring state. The advantage of this setting is that the collision state of the robotic arm can be detected without using high-precision sensors such as force sensors, resulting in better economic efficiency.

[0071] S250. Based on the current working state, current difference, target inherent parameters of the target redundant joint, preset motor torque constant corresponding to the target robotic arm, current redundant joint position, and current redundant joint speed, determine the target redundant joint acceleration of the target redundant joint.

[0072] The target's inherent parameters include: the virtual mass, damping, and stiffness coefficients of the target's redundant joints.

[0073] Specifically, if the current operating state is a collision state, the target redundant joint acceleration is determined using admittance control theory (pre-set linear equations), current difference, target inherent parameters of the target redundant joint, preset motor torque constant corresponding to the target robotic arm, current redundant joint position, and current redundant joint velocity. By controlling the motion of the target redundant joint through admittance control theory, the relationship between the redundant joint motion and the collision force can be established. That is, it will respond with different speeds according to the severity of the robotic arm collision, exhibiting strong adaptability.

[0074] Based on the above technical solution, "determining the target redundant joint acceleration based on the current working state, current difference, target inherent parameters of the target redundant joint, preset motor torque constant corresponding to the target robotic arm, current redundant joint position, and current redundant joint speed" can include: multiplying the current difference by the preset motor torque constant corresponding to the target robotic arm to obtain a first multiplication result; multiplying the damping of the target redundant joint by the current redundant joint speed to obtain a second multiplication result; multiplying the stiffness coefficient of the target redundant joint by the current redundant joint position to obtain a third multiplication result; summing the second and third multiplication results to obtain an addition result; subtracting the first multiplication result from the addition result to obtain a subtraction result; and determining the ratio of the subtraction result to the virtual mass of the target redundant joint as the target redundant joint acceleration.

[0075] Specifically, the current difference ΔI r Preset motor torque constant K corresponding to the target robotic arm m Multiply to obtain the first multiplication result (K) m ΔI r The damping B of the target redundant joint is compared with the current velocity of the redundant joint. Multiply to get the second product result. The stiffness coefficient K of the target redundant joint is compared with the current redundant joint position θ. r Multiplying them together yields the third multiplication result (Kθ). r ); multiply the result of the second multiplication The result of multiplying with the third (Kθ) r Summing them together yields the result. The result of the first multiplication (K) m ΔI r ) and the sum of the results Subtract to get the result of the subtraction. Subtract the result The ratio of the virtual mass M of the target redundant joint to the target redundant joint's virtual mass M is determined as the target redundant joint acceleration. in,

[0076] S260. By integrating the acceleration of the target redundant joints, the target pose matrix of the target redundant joints is obtained.

[0077] Specifically, the target redundant joint acceleration is integrated once using integrator ∫ to obtain the target redundant joint velocity, and the target redundant joint position is obtained by integrating the target redundant joint velocity again using integrator ∫. Based on the target redundant joint position, the position-related matrices in the current pose matrix are adjusted to obtain the target pose matrix of the target redundant joint.

[0078] It should be noted that after determining the target pose matrix of the redundant joint, the current pose matrix of the redundant joint can be updated using this matrix. Using the updated current pose matrix, the target robotic arm control commands, and the corresponding preset inverse kinematics equations, the target execution joint position is determined, and the reference execution joint position is updated using this target execution joint position. The target robotic arm controls the redundant joint to move to the updated current pose matrix and simultaneously controls the execution joint to move to the updated reference execution joint position. It continues to monitor the current working state of the target robotic arm, effectively mitigating collisions and ensuring that the end effector pose of the target robotic arm does not deviate—that is, it reaches the desired end effector position. This further enables accurate control of the target robotic arm, and in the event of a collision, timely adjustments are made according to the updated pose and position, effectively preventing further escalation of the collision. This ensures that the target robotic arm can reproduce the control commands from the operating end while simultaneously eliminating the collision. This means that the control precision of the robotic arm will not be lost, nor will the robotic arm stop moving due to power failure. Furthermore, through the design of redundant degrees of freedom (redundant joints), the target robotic arm can quickly adjust its posture after a collision, avoiding further damage.

[0079] For example, when the target robotic arm has just collided with something, the collision information is displayed on the user's screen to alert the user to the collision. The displayed content may include, but is not limited to, graphics, icons, animations, colors, and voice prompts.

[0080] The technical solution of this invention determines the current working state of the target robotic arm based on the current difference between the current working current and the reference working current. The current working state indicates whether the target robotic arm will collide. If the current working state indicates that a collision will occur, the target redundant joint acceleration is determined based on the current difference, the target inherent parameters of the target redundant joint, the preset motor torque constant corresponding to the target robotic arm, the current position of the redundant joint, and the current velocity of the redundant joint. By integrating the target redundant joint acceleration, the target pose matrix of the target redundant joint is accurately determined without stopping the movement of the robotic arm. Then, when the robotic arm collides, the determined target pose matrix is ​​used to adjust the target redundant joint in a timely manner, and the collision is effectively resolved when the robotic arm collides.

[0081] The following are embodiments of the joint pose determination device for a robotic arm provided in this invention. This device and the joint pose determination method for a robotic arm in the above embodiments belong to the same inventive concept. For details not described in detail in the embodiments of the joint pose determination device for a robotic arm, please refer to the embodiments of the joint pose determination method for a robotic arm described above.

[0082] Example 3

[0083] Figure 3 This is a schematic diagram of the joint pose determination device for a robotic arm provided in Embodiment 3 of the present invention. Figure 3 As shown, the device includes: an information acquisition module 310, a reference execution joint position determination module 320, a reference operating current determination module 330, and a target pose matrix determination module 340.

[0084] The information acquisition module 310 is used to acquire the current pose matrix of the target redundant joint in the target robot and the target manipulator control command triggered by the user. The target manipulator of the target robot includes: target redundant joint and target execution joint. The reference execution joint position determination module 320 is used to determine the reference execution joint position of the target execution joint based on the current pose matrix, the target manipulator control command and the preset inverse kinematics equation corresponding to the target manipulator. The reference operating current determination module 330 is used to determine the reference operating current of the target redundant joint based on the current pose matrix, the reference execution joint position and the preset manipulator dynamics model corresponding to the target manipulator. The target pose matrix determination module 340 is used to determine the target pose matrix of the target redundant joint based on the current difference between the current operating current and the reference operating current.

[0085] The technical solution of this invention involves acquiring the current pose matrix of the target redundant joint in the target robot and the user-triggered target robotic arm control command. The target robotic arm of the target robot includes a target redundant joint and a target execution joint. Based on the current pose matrix, the target robotic arm control command, and the preset inverse kinematics equation corresponding to the target robotic arm, the reference execution joint position of the target execution joint is determined. When the target robot controls the target execution joint to move to the reference execution joint position, the target redundant joint generates a current operating current. Based on the current pose matrix, the reference execution joint position, and the preset robotic arm dynamics model corresponding to the target robotic arm, the reference operating current of the target redundant joint is determined. Based on the current difference between the current and the reference operating current, the target pose matrix of the target redundant joint can be accurately and conveniently determined. This allows for timely adjustment of the target redundant joint using the determined target pose matrix when the robotic arm collides, thereby effectively resolving the collision.

[0086] Based on the above technical solution, the information acquisition module 310 is specifically used to: acquire the current redundant joint position of the target redundant joint in the target robot; determine the current pose matrix of the target redundant joint based on the current redundant joint position and the preset pose matrix calculation formula corresponding to the target redundant joint; wherein, the preset pose matrix calculation formula is the pose matrix calculation formula of rotating around a preset direction in the target coordinate system.

[0087] Based on the above technical solution, the reference joint position determination module 320 is specifically used to: determine the reference motion command of the target joint based on the current pose matrix and the target robotic arm control command; and determine the reference joint position of the target joint based on the reference motion command and the preset inverse kinematic equation corresponding to the target robotic arm.

[0088] Based on the above technical solution, the reference operating current determination module 330 may include:

[0089] The current redundant joint position determination submodule is used to extract the position of the target redundant joint by extracting the position from the current pose matrix;

[0090] The reference operating current determination submodule is used to determine the reference operating current of the target redundant joint based on the current redundant joint position, the reference execution joint position, the preset motor torque constant corresponding to the target manipulator, and the preset manipulator dynamics model corresponding to the target manipulator.

[0091] The preset robotic arm dynamics model is constructed based on the preset robotic arm dynamics equations corresponding to the target robotic arm.

[0092] Based on the above technical solution, the reference operating current determination submodule is specifically used for: obtaining the current redundant joint velocity and current redundant joint acceleration of the target redundant joint by differentiating the current redundant joint position; obtaining the reference execution joint velocity and reference execution joint acceleration of the target execution joint by differentiating the reference execution joint position; inputting the current redundant joint position, current redundant joint velocity, current redundant joint acceleration, reference execution joint position, reference execution joint velocity, reference execution joint acceleration, and the preset motor torque constant corresponding to the target robotic arm into the preset robotic arm dynamics model corresponding to the target robotic arm, and determining the reference operating current of the target redundant joint based on the output of the preset robotic arm dynamics model.

[0093] Based on the above technical solution, the target pose matrix determination module 340 may include:

[0094] The current working state determination submodule is used to determine the current working state of the target robotic arm based on the current difference between the current working current and the reference working current.

[0095] The target redundant joint acceleration determination submodule is used to determine the target redundant joint acceleration based on the current working state, current difference, target inherent parameters of the target redundant joint, preset motor torque constant corresponding to the target robotic arm, current redundant joint position, and current redundant joint speed; the target inherent parameters include: virtual mass, damping, and stiffness coefficient of the target redundant joint;

[0096] The target pose matrix determination submodule is used to obtain the target pose matrix of the target redundant joints by integrating the acceleration of the target redundant joints.

[0097] Based on the above technical solution, the target redundant joint acceleration determination submodule is specifically used for: multiplying the current difference with the preset motor torque constant corresponding to the target robotic arm to obtain the first multiplication result; multiplying the damping of the target redundant joint with the current redundant joint velocity to obtain the second multiplication result; multiplying the stiffness coefficient of the target redundant joint with the current redundant joint position to obtain the third multiplication result; summing the second and third multiplication results to obtain the addition result; subtracting the first multiplication result from the addition result to obtain the subtraction result; and determining the ratio of the subtraction result to the virtual mass of the target redundant joint as the target redundant joint acceleration.

[0098] The joint pose determination device for a robotic arm provided in this embodiment of the invention can execute the joint pose determination method for a robotic arm provided in any embodiment of the invention, and has the corresponding functional modules and beneficial effects for executing the joint pose determination method for a robotic arm.

[0099] It is worth noting that in the above embodiments for determining the joint pose of the robotic arm, the various units and modules included are only divided according to functional logic, but are not limited to the above division, as long as the corresponding functions can be achieved; in addition, the specific names of each functional unit are only for easy differentiation and are not used to limit the scope of protection of the present invention.

[0100] Example 4

[0101] Figure 4 A schematic diagram of an electronic device 10 that can be used to implement embodiments of the present invention is shown. The electronic device is intended to represent various forms of digital computers, such as laptop computers, desktop computers, workstations, personal digital assistants, servers, blade servers, mainframe computers, and other suitable computers. The electronic device can also represent various forms of mobile devices, such as personal digital processors, cellular phones, smartphones, wearable devices (e.g., helmets, glasses, watches, etc.), and other similar computing devices. The components shown herein, their connections and relationships, and their functions are merely illustrative and are not intended to limit the implementation of the invention described and / or claimed herein.

[0102] like Figure 4 As shown, the electronic device 10 includes at least one processor 11 and a memory, such as a read-only memory (ROM) 12 or a random access memory (RAM) 13, communicatively connected to the at least one processor 11. The memory stores computer programs executable by the at least one processor. The processor 11 can perform various appropriate actions and processes based on the computer program stored in the ROM 12 or loaded from storage unit 18 into the RAM 13. The RAM 13 may also store various programs and data required for the operation of the electronic device 10. The processor 11, ROM 12, and RAM 13 are interconnected via a bus 14. An input / output (I / O) interface 15 is also connected to the bus 14.

[0103] Multiple components in electronic device 10 are connected to I / O interface 15, including: input unit 16, such as keyboard, mouse, etc.; output unit 17, such as various types of displays, speakers, etc.; storage unit 18, such as disk, optical disk, etc.; and communication unit 19, such as network card, modem, wireless transceiver, etc. Communication unit 19 allows electronic device 10 to exchange information / data with other devices through computer networks such as the Internet and / or various telecommunications networks.

[0104] Processor 11 can be a variety of general-purpose and / or special-purpose processing components with processing and computing capabilities. Some examples of processor 11 include, but are not limited to, a central processing unit (CPU), a graphics processing unit (GPU), various special-purpose artificial intelligence (AI) computing chips, various processors running machine learning model algorithms, a digital signal processor (DSP), and any suitable processor, controller, microcontroller, etc. Processor 11 performs the various methods and processes described above, such as the joint pose determination method for a robotic arm.

[0105] In some embodiments, the joint pose determination method for the robotic arm can be implemented as a computer program tangibly contained in a computer-readable storage medium, such as storage unit 18. In some embodiments, part or all of the computer program can be loaded and / or installed on electronic device 10 via ROM 12 and / or communication unit 19. When the computer program is loaded into RAM 13 and executed by processor 11, one or more steps of the joint pose determination method for the robotic arm described above can be performed. Alternatively, in other embodiments, processor 11 can be configured to perform the joint pose determination method for the robotic arm by any other suitable means (e.g., by means of firmware).

[0106] Various embodiments of the systems and techniques described above herein can be implemented in digital electronic circuit systems, integrated circuit systems, field-programmable gate arrays (FPGAs), application-specific integrated circuits (ASICs), application-specific standard products (ASSPs), systems-on-a-chip (SoCs), payload-programmable logic devices (CPLDs), computer hardware, firmware, software, and / or combinations thereof. These various embodiments may include implementations in one or more computer programs that can be executed and / or interpreted on a programmable system including at least one programmable processor, which may be a dedicated or general-purpose programmable processor, capable of receiving data and instructions from a storage system, at least one input device, and at least one output device, and transmitting data and instructions to the storage system, the at least one input device, and the at least one output device.

[0107] Computer programs used to implement the methods of the present invention may be written in any combination of one or more programming languages. These computer programs may be provided to a processor of a general-purpose computer, a special-purpose computer, or other programmable data processing device, such that when executed by the processor, the computer programs cause the functions / operations specified in the flowcharts and / or block diagrams to be performed. The computer programs may be executed entirely on a machine, partially on a machine, or as a standalone software package, partially on a machine and partially on a remote machine, or entirely on a remote machine or server.

[0108] In the context of this invention, a computer-readable storage medium can be a tangible medium that may contain or store a computer program for use by or in conjunction with an instruction execution system, apparatus, or device. A computer-readable storage medium may include, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatus, or devices, or any suitable combination thereof. Alternatively, a computer-readable storage medium may be a machine-readable signal medium. More specific examples of machine-readable storage media include electrical connections based on one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fibers, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination thereof.

[0109] To provide interaction with a user, the systems and techniques described herein can be implemented on an electronic device having: a display device for displaying information to the user (e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor); and a keyboard and pointing device (e.g., a mouse or trackball) through which the user provides input to the electronic device. Other types of devices can also be used to provide interaction with the user; for example, feedback provided to the user can be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and input from the user can be received in any form (including sound input, voice input, or tactile input).

[0110] The systems and technologies described herein can be implemented in computing systems that include backend components (e.g., as data servers), or computing systems that include middleware components (e.g., application servers), or computing systems that include frontend components (e.g., user computers with graphical user interfaces or web browsers through which users can interact with implementations of the systems and technologies described herein), or any combination of such backend, middleware, or frontend components. The components of the system can be interconnected via digital data communication of any form or medium (e.g., communication networks). Examples of communication networks include local area networks (LANs), wide area networks (WANs), blockchain networks, and the Internet.

[0111] A computing system can include clients and servers. Clients and servers are generally located far apart and typically interact through a communication network. The client-server relationship is created by computer programs running on the respective computers and having a client-server relationship with each other. The server can be a cloud server, also known as a cloud computing server or cloud host, which is a hosting product within the cloud computing service system to address the shortcomings of traditional physical hosts and VPS services, such as high management difficulty and weak business scalability.

[0112] This invention also provides a computer program product, including a computer program that, when executed by a processor, implements the joint pose determination method for a robotic arm as provided in any embodiment of this application.

[0113] In implementing the computer program product, computer program code for performing the operations of this invention can be written in one or more programming languages ​​or a combination thereof. Programming languages ​​include object-oriented programming languages ​​such as Java, Smalltalk, and C++, as well as conventional procedural programming languages ​​such as C or similar languages. The program code can be executed entirely on the user's computer, partially on the user's computer, as a standalone software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In cases involving remote computers, the remote computer can be connected to the user's computer via any type of network—including a local area network (LAN) or a wide area network (WAN)—or can be connected to an external computer (e.g., via the Internet using an Internet service provider). This program product belongs to the same inventive concept as the joint pose determination method for the robotic arm disclosed in the embodiments of this application, and therefore will not be described further here.

[0114] It should be understood that the various forms of processes shown above can be used, with steps reordered, added, or deleted. For example, the steps described in this invention can be executed in parallel, sequentially, or in different orders, as long as the desired result of the technical solution of this invention can be achieved, and this is not limited herein.

[0115] The specific embodiments described above do not constitute a limitation on the scope of protection of this invention. Those skilled in the art should understand that various modifications, combinations, sub-combinations, and substitutions can be made according to design requirements and other factors. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this invention should be included within the scope of protection of this invention.

Claims

1. A method for determining the joint pose of a robotic arm, characterized in that, The method comprises the following steps: obtaining a current pose matrix of a target redundant joint in a target robot and a target robot control instruction triggered by a user; the target robot of the target robot comprises a target redundant joint and a target execution joint; based on the current pose matrix, the target robot control instruction and the preset inverse kinematics equation corresponding to the target robot, the reference execution joint position of the target execution joint is determined; based on the current pose matrix, the reference execution joint position and the preset robot dynamics model corresponding to the target robot, the reference working current of the target redundant joint is determined; based on the current working current and the reference working current, the target pose matrix of the target redundant joint is determined; the current working current is the current generated by the target redundant joint when the target execution joint moves to the reference execution joint position; wherein, based on the current working current and the reference working current, the target pose matrix of the target redundant joint is determined, comprising: based on the current working current and the reference working current, the current working state of the target robot is determined; based on the current working state, the current working current, the target intrinsic parameter of the target redundant joint, the preset motor torque constant corresponding to the target robot, the current redundant joint position and the current redundant joint speed, the target redundant joint acceleration of the target redundant joint is determined; the target intrinsic parameter comprises: the virtual mass, damping and stiffness coefficient of the target redundant joint; the target pose matrix of the target redundant joint is obtained by integrating the target redundant joint acceleration; wherein, based on the current working state, the current working current, the target intrinsic parameter of the target redundant joint, the preset motor torque constant corresponding to the target robot, the current redundant joint position and the current redundant joint speed, the target redundant joint acceleration of the target redundant joint is determined, comprising: multiplying the current working current and the preset motor torque constant corresponding to the target robot to obtain a first multiplication result; multiplying the damping of the target redundant joint and the current redundant joint speed to obtain a second multiplication result; multiplying the stiffness coefficient of the target redundant joint and the current redundant joint position to obtain a third multiplication result; summing the second multiplication result and the third multiplication result to obtain a sum result; subtracting the first multiplication result from the sum result to obtain a subtraction result; the ratio of the subtraction result and the virtual mass of the target redundant joint is determined as the target redundant joint acceleration of the target redundant joint.

2. The method of claim 1, wherein, The method comprises the following steps: obtaining a current pose matrix of a target redundant joint in a target robot and a target robot control instruction triggered by a user; the target robot of the target robot comprises a target redundant joint and a target execution joint; based on the current pose matrix, the reference execution joint position and the preset robot dynamics model corresponding to the target robot, the reference working current of the target redundant joint is determined; based on the current pose matrix, the reference execution joint position and the preset robot dynamics model corresponding to the target robot, the reference working current of the target redundant joint is determined; wherein, the preset pose matrix calculation formula is a pose matrix calculation formula of rotating around a preset direction in a target coordinate system.

3. The method of claim 1, wherein, The method comprises the following steps: determining the reference joint position of the target execution joint based on the current pose matrix, the target robot control instruction and the preset inverse kinematics equation corresponding to the target robot; determining the reference joint position of the target execution joint based on the reference motion instruction and the preset inverse kinematics equation corresponding to the target robot.

4. The method of claim 1, wherein, The method comprises the following steps: extracting the current redundant joint position of the target redundant joint from the current pose matrix; determining the reference working current of the target redundant joint based on the current redundant joint position, the reference joint position, the preset motor torque constant corresponding to the target robot and the preset robot dynamics model corresponding to the target robot. The preset robot dynamics model is constructed based on the preset robot dynamics equation corresponding to the target robot.

5. The method of claim 4, wherein, The method comprises the following steps: deriving the current redundant joint speed and the current redundant joint acceleration of the target redundant joint from the current redundant joint position; deriving the reference execution joint speed and the reference execution joint acceleration of the target execution joint from the reference joint position; inputting the current redundant joint position, the current redundant joint speed, the current redundant joint acceleration, the reference joint position, the reference execution joint speed, the reference execution joint acceleration and the preset motor torque constant corresponding to the target robot into the preset robot dynamics model corresponding to the target robot, and determining the reference working current of the target redundant joint based on the output of the preset robot dynamics model.

6. A joint pose determination device of a robot arm, characterized by, The device comprises: an information acquisition module configured to acquire the current pose matrix and the current working current of a target redundant joint in a target robot, and a target robot control instruction triggered by a user; the target robot comprises a target redundant joint and a target execution joint; a reference joint position determination module configured to determine the reference joint position of the target execution joint based on the current pose matrix, the target robot control instruction and the preset inverse kinematics equation corresponding to the target robot; a reference working current determination module configured to determine the reference working current of the target redundant joint based on the current pose matrix, the reference joint position and the preset robot dynamics model corresponding to the target robot. A target pose matrix determination module is configured to determine a target pose matrix of the target redundant joint based on a current difference between the current working current and the reference working current. The target pose matrix determination module includes: a current working state determination submodule configured to determine a current working state of the target robot arm based on the current difference between the current working current and the reference working current; a target redundant joint acceleration determination submodule configured to determine a target redundant joint acceleration of the target redundant joint based on the current working state, the current difference, a target intrinsic parameter of the target redundant joint, a preset motor torque constant corresponding to the target robot arm, a current redundant joint position, and a current redundant joint velocity; the target intrinsic parameter includes a virtual mass, a damping, and a stiffness coefficient of the target redundant joint; and a target pose matrix determination submodule configured to obtain the target pose matrix of the target redundant joint by performing integral processing on the target redundant joint acceleration. The target redundant joint acceleration determination submodule is specifically configured to: multiply the current difference and the preset motor torque constant corresponding to the target robot arm to obtain a first multiplication result; multiply the damping of the target redundant joint and the current redundant joint velocity to obtain a second multiplication result; multiply the stiffness coefficient of the target redundant joint and the current redundant joint position to obtain a third multiplication result; sum the second multiplication result and the third multiplication result to obtain an addition result; subtract the first multiplication result from the addition result to obtain a subtraction result; and determine a ratio of the subtraction result and the virtual mass of the target redundant joint as the target redundant joint acceleration of the target redundant joint.

7. An electronic device, comprising: The electronic device includes: one or more processors; a memory for storing one or more programs; when the one or more programs are executed by the one or more processors, the one or more processors implement the joint pose determination method of the robot arm as claimed in any one of claims 1-5.

8. A computer-readable storage medium having stored thereon a computer program, characterized in that, The program is executed by the processor to implement the joint pose determination method of the robot arm as claimed in any one of claims 1-5.

Images