Robot master hand joint angle determination method and device, robot, and medium

Abstract

The application discloses a robot master hand joint angle determination method and device, a robot and a medium. The method comprises the following steps: determining an interpolation pose based on a quaternion spherical surface posture interpolation method; determining a first pose offset of the interpolation pose compared with a first pose, a first joint angle combination corresponding to the first pose; determining a second joint angle combination corresponding to the first pose offset; determining a third joint angle combination according to the second joint angle combination and the first joint angle combination, and a pose difference between a third pose corresponding to the third joint angle combination and the interpolation pose; if the third pose does not meet a second accuracy condition, taking the pose difference as the first pose offset, taking the third joint angle combination as the first angle combination, returning to the step of determining the second joint angle combination, and repeating the steps until the third pose meets the second accuracy condition, and taking the third joint angle combination as a target joint angle combination. The application can improve the accuracy of robot master hand joint angle determination.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robots, and in particular to a robot master hand joint angle determination method and device, a robot, and a medium. BACKGROUND

[0002] A seven-axis robot, also known as a redundant robot, has one more additional axis than a six-axis robot. The additional axis allows the robot to avoid certain targets, facilitating the end effector to reach a specific position and enabling more flexible adaptation to some special working environments.

[0003] Kinematics solving is the basis of mechanical arm control, and kinematics inverse solution is the process of solving the joint angles of a mechanical arm according to the target pose of the end of the mechanical arm. According to different modeling methods, kinematics inverse solution can be divided into analytical method and numerical method.

[0004] Since the degree of freedom of the mechanical arm of a redundant robot is greater than the dimension of the Cartesian space, theoretically, its kinematics inverse solution has infinitely many solutions, and it is difficult to find the optimal solution using the analytical method. The numerical method usually uses an iterative method, but the existing iterative method has the problems of long time consumption and poor real-time performance. SUMMARY

[0005] The present application provides a robot master hand joint angle determination method and device, a robot, and a medium to solve the problem of slow determination of the joint angle of the master hand of the existing robot.

[0006] According to an aspect of the present application, a robot master hand joint angle determination method is provided for controlling a seven-axis robot, the seven-axis robot including a master hand and a slave arm, the master hand including seven axes including:

[0007] If the first pose of the master hand does not meet the first accuracy condition, an interpolated pose is determined based on a quaternion spherical pose interpolation method according to the first pose, the target pose, the master hand motion direction, and the slave arm motion direction;

[0008] The offset of the interpolated pose compared to the first pose is taken as a first pose offset, and the combination of the joint angles corresponding to the first pose is taken as a first joint angle combination, wherein a joint is used to connect two adjacent axes;

[0009] A second joint angle combination corresponding to the first pose offset is determined based on the first pose offset and the first joint angle combination;

[0010] A third joint angle combination and a third pose corresponding to the third joint angle combination are determined according to the second joint angle combination and the first joint angle combination;

[0011] determine a pose difference between the third pose and the interpolated pose, and determine whether the third pose meets a second accuracy condition based on the pose difference;

[0012] If no, take the pose difference as the first pose offset, take the third joint angle combination as a first angle combination, and return the step of determining a second joint angle combination corresponding to the first pose offset based on the first pose offset and the first joint angle combination until the third pose meets the second accuracy condition, and take the third joint angle combination as a target joint angle combination.

[0013] According to another aspect of the present application, there is provided a robot master hand joint angle determination device, comprising:

[0014] an interpolated pose module configured to, if a first pose of the master hand does not meet a first accuracy condition, determine an interpolated pose based on a quaternion spherical pose interpolation method according to the first pose, a target pose, a master hand movement direction, and a slave arm movement direction;

[0015] a pose offset module configured to take an offset of the interpolated pose relative to the first pose as a first pose offset, and take a combination of joint angles corresponding to the first pose as a first joint angle combination, wherein a joint is configured to connect two adjacent axes;

[0016] an inverse solution module configured to determine a second joint angle combination corresponding to the first pose offset based on the first pose offset and the first joint angle combination;

[0017] a third pose module configured to determine a third joint angle combination and a third pose corresponding to the third joint angle combination according to the second joint angle combination and the first joint angle combination;

[0018] a pose difference module configured to determine a pose difference between the third pose and the interpolated pose, and determine whether the third pose meets a second accuracy condition based on the pose difference;

[0019] an iteration module configured to, if no, take the pose difference as the first pose offset, take the third joint angle combination as a first angle combination, and return the step of determining a second joint angle combination corresponding to the first pose offset based on the first pose offset and the first joint angle combination until the third pose meets the second accuracy condition, and take the third joint angle combination as a target joint angle combination.

[0020] According to another aspect of the present application, there is provided a surgical robot, comprising:

[0021] a master hand and a slave arm;

[0022] at least one processor connected to the master hand and the slave arm; and

[0023] a memory connected in communication with the at least one processor; wherein

[0024] the memory stores a computer program executable by the at least one processor, and the computer program is executed by the at least one processor to enable the at least one processor to perform the robot master hand joint angle determination method according to any one of the embodiments of the present application.

[0025] According to another aspect of the present application, there is provided a computer readable storage medium storing computer instructions for enabling a processor to implement the robot master hand joint angle determination method according to any one of the embodiments of the present application when executed by the processor.

[0026] The technical solution of the embodiments of the present application is based on the quaternion spherical surface pose interpolation method, and the interpolation pose is determined according to the first pose, the target pose, the master hand movement direction, and the slave arm movement direction, which improves the rationality of the interpolation pose and the speed of approaching the target pose. The first pose offset is updated through the pose difference between the third pose and the interpolation pose, and the first joint angle combination is updated through the third joint angle combination, and the third pose is updated until the third pose meets the second accuracy condition by taking the first pose offset and the first joint angle combination as iteration parameters, which improves the accuracy of the third pose, and thus improves the accuracy of the target joint angle combination, i.e. the accuracy of the first pose inverse solution.

[0027] It should be understood that the content described in this part is not intended to identify key or important features of the embodiments of the present application, nor is it used to limit the scope of the present application. Other features of the present application will become apparent from the following description. BRIEF DESCRIPTION OF DRAWINGS

[0028] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings needed in the embodiment description will be briefly introduced below. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can also be obtained by those skilled in the art without creative labor.

[0029] Figure 1 is a flowchart of a robot joint angle determination method according to an embodiment of the present application;

[0030] Figure 2 is still another flowchart of a robot joint angle determination method according to an embodiment of the present application;

[0031] Figure 3 is a structural schematic diagram of a robot joint angle determination device provided according to an embodiment of the present application;

[0032] Figure 4 is a structural schematic diagram of a robot for implementing a robot joint angle determination method according to an embodiment of the present application. DETAILED DESCRIPTION

[0033] In order to make the personnel in the technical field better understand the present application scheme, the technical scheme in the embodiment of the present application will be described clearly and completely below in combination with the drawings in the embodiment of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, but not all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by the person skilled in the art without creative labor should belong to the scope of protection of the present application.

[0034] It should be noted that the terms "first", "second" and the like in the specification and claims of the present application and the above-mentioned drawings are used to distinguish similar objects, and do not necessarily indicate a specific order or sequence. It should be understood that the data thus used can be interchanged under appropriate circumstances, so that the embodiments of the present application described herein can be implemented in an order other than that illustrated or described herein. In addition, the terms "include" and "have" and any variations thereof are intended to cover non-exclusive inclusion, for example, a process, method, system, product or device including a series of steps or units does not necessarily have to be limited to those steps or units clearly listed, but can include other steps or units not clearly listed or inherent to these processes, methods, products or devices.

[0035] Figure 1 A flowchart of a robot master hand joint angle determination method provided by the embodiment of the present application, the embodiment can be applicable to the case of the robot master hand joint angle determination method, the method can be executed by a robot master hand joint angle determination device, the robot master hand joint angle determination device can be realized in the form of hardware and / or software, and the robot master hand joint angle determination device can be configured in a robot processor. As shown in the figure, the method comprises: Figure 1

[0036] S110, if the first pose of the master hand does not meet the first accuracy condition, determining an interpolation pose based on the first pose, the target pose, the master hand motion direction and the slave arm motion direction according to the quaternion spherical pose interpolation method.

[0037] In one embodiment, the robot is a surgical robot, which includes a master hand and a slave arm connected to the master hand, and the master hand can control the motion of the slave arm. The surgical instrument is installed at the end of the slave arm.

[0038] ​The first pose is a current pose of the master hand, and corresponds to a first joint angle combination, which includes angles of each joint of the master hand at a current time. The target pose is a desired pose of the master hand. It can be understood that the angles of each joint of the master hand are determined, and the pose of the master hand is determined.

[0039] The processor obtains the first pose, the target pose, the master hand movement direction and the slave arm movement direction, determines a difference between a matrix corresponding to the first pose and a matrix corresponding to the target pose, and the difference is a target pose offset. If the target pose offset is less than a set threshold, it is determined that the first pose meets the first accuracy condition, and therefore the inverse solution of the target pose is determined, and a joint angle combination included in the inverse solution is taken as the inverse solution of the first pose. If the target pose offset is greater than or equal to the set threshold, it is determined that the first pose does not meet the first accuracy condition, and therefore an interpolation pose is determined based on the quaternion spherical pose interpolation method according to the first pose, the target pose, the master hand movement direction and the slave arm movement direction, so that the master hand pose is expected to approach the target pose through one or more interpolations. Specifically, the first pose is taken as a starting point, and an interpolation pose is determined based on the relationship between the master hand movement direction and the slave arm movement direction and the quaternion spherical pose interpolation method along a direction from the first pose to the target pose. It can be understood that a pose offset between the interpolation pose and the first pose is less than a pose offset between the target pose and the first pose.

[0040] In one embodiment, if the master hand movement direction and the slave arm movement direction are in the same direction, an interpolation pose is determined based on a pose difference between the target pose and the first pose along a direction from the first pose to the target pose using the quaternion spherical pose interpolation method; if the master hand movement direction and the slave arm movement direction are in different directions, an interpolation pose is determined based on a set movement step of the slave arm and the quaternion spherical pose interpolation method along a direction from the first pose to the target pose. This embodiment uses the quaternion spherical pose interpolation method with direction to solve the problem that the existing pose interpolation method does not consider the interpolation direction, and increases the synchronization constraint of master-slave movement.

[0041] Specifically, if the master hand movement direction and the slave arm movement direction are in the same direction, a pose difference between the target pose and the first pose is determined, a product of the pose difference and a set value is determined, and the product is taken as an interpolation step, wherein the set value is greater than 1 and less than 2. This interpolation step setting method can make the master hand play a chasing role, and quickly synchronize with the slave arm.

[0042] If the master hand movement direction and the slave arm movement direction are different, a set movement step of the slave arm is taken as an interpolation step, and an interpolation pose is determined based on the interpolation step and the quaternion spherical pose interpolation method, so as to achieve the synchronous movement control between the master hand and the slave arm while reducing the pose offset from the target pose.

[0043] In one embodiment, a standard D-H method is adopted to establish a kinematic model of the robot, to obtain D-H parameters of each joint of the master hand of the robot, wherein the D-H parameters include d i , a i , a i , q i and q i , wherein i = (1, 2,..., 7); d i is a link offset of the ith link, representing a distance of translating X i-1 along Z i-1 axis to X i; a i is a link length of the ith link, representing a distance of translating Z i-1 along X i axis to Z i; a i represents a link twist angle of the ith link, representing an angle of rotating Z i-1 along X i axis to Z i; q i represents a joint angle of the ith joint, and q i is an included angle between the ith joint and the ith+1 joint. The D-H parameters of each joint can be referred to the following D-H parameter table:

[0044] D-H parameter table

[0045] i d i ]]> θ i ]]> i ]]> ​ a i ]]> 1 0 0+q1 π / 2 -16 2 -3.2 π / 2+q2 0 280 3 0 -π / 2+q3 -π / 2 320 4 164 π / 2+q4 π / 2 0 5 0 0+q5 -π / 2 0 6 0 -π / 2+q6 π / 2 0 7 0 0+q7 π / 2 0

[0046] According to the D-H modeling method, the transformation matrix from the ith joint to the ith+1 joint is:

[0047]

[0048] The forward position matrix TM of the master hand can be represented as:

[0049]

[0050] The target position corresponding matrix is Specifically, it can be represented as:

[0051]

[0052] Wherein, n x , n y , n z are decomposition information of the normal vector of the slave arm in x, y, z directions, o x , o y , o z are decomposition information of the direction vector of the slave arm in x, y, z directions; a x , a y , a z are decomposition information of the approach vector of the slave arm in x, y, z directions, p x , p y , p zThe decomposition information of the origin vector of the slave arm in the x, y, and z directions corresponds to the first pose.

[0053] If the first pose is represented as The target pose offset of the target pose relative to the first pose can be represented as:

[0054]

[0055] If the target pose offset is less than or equal to a set offset threshold, it is determined that the first pose meets the first accuracy condition, and therefore the target pose is used for inverse kinematics solving, that is, The inverse solution of the first pose is represented.

[0056] If the target pose offset is greater than the set offset threshold, based on the quaternion spherical pose interpolation method, the pose interpolation matrix, that is, the interpolation pose, is calculated from the rotation part of the matrix in the direction of . Wherein, The rotation part of and is the corresponding data of the first three rows and the first three columns respectively. The interpolation pose can be represented as:

[0057]

[0058] Wherein, f inp represents the interpolation algorithm.

[0059] S120, the offset of the interpolation pose relative to the first pose is taken as the first pose offset, and the combination of the joint angles corresponding to the first pose is taken as the first joint angle combination, wherein the joint is used to connect two adjacent axes.

[0060] After the interpolation pose is determined, the pose offset of the interpolation pose relative to the first pose is determined, and the pose offset is taken as the first pose offset. Wherein, the first pose offset can be represented as:

[0061]

[0062] Wherein, is the inverse matrix of the matrix corresponding to the first pose.

[0063] It can be understood that one pose corresponds to one joint angle combination. In this embodiment, the combination of the joint angles corresponding to the first pose is taken as the first joint angle combination, and the first joint angle combination is represented as q.

[0064] The master hand in the embodiment includes seven axes, and adjacent two axes are connected through a corresponding joint.

[0065] In S130, a second joint angle combination corresponding to the first pose offset is determined based on the first pose offset and the first joint angle combination.

[0066] In the master-slave control type robot, the inverse solution is used to make the processor determine the angles of the joints of the master hand in the corresponding pose. The inverse solution corresponding to the first pose offset includes a second joint angle combination, which is used to make the master hand determine the angle data of each joint generated by the corresponding interpolation operation, or in other words, the angle change data of each joint that can be caused by the interpolation pose.

[0067] In one embodiment, a Jacobian pseudo-inverse matrix is determined according to the first joint angle combination, and then a second joint angle combination including the movement angles of the joints is determined based on the Jacobian pseudo-inverse matrix and the first pose offset. Wherein, the Jacobian matrix corresponding to the first joint angle combination is determined by using the prior art, which is not specifically described in this embodiment.

[0068] After the Jacobian matrix is determined, the Jacobian pseudo-inverse matrix is determined by the following formula:

[0069] J + =J T *(J*J T ) -1

[0070] Wherein, J is the Jacobian matrix, J + is the Jacobian pseudo-inverse matrix. The product of the Jacobian pseudo-inverse matrix and the first pose offset is taken as the second joint angle combination, and the specific formula is as follows:

[0071]

[0072] Wherein, e is the first pose offset.

[0073] In S140, a third joint angle combination and a third pose corresponding to the third joint angle combination are determined according to the second joint angle combination and the first joint angle combination.

[0074] The interpolation pose is a pose determined by interpolation based on the first pose and the target pose, and the third pose is a pose determined by the inverse solution result based on the first joint angle corresponding to the first pose and the second joint combination corresponding to the first pose offset. Therefore, the third pose can be understood as the optimization result of the interpolation pose.

[0075] After the first joint angle combination and the second joint angle combination are determined, the sum of the angles of the joints in the first joint angle combination and the corresponding joint angles in the second joint angle combination is determined to obtain a third joint angle combination. Specifically, the second joint angle combination is expressed as Then the third joint angle combination can be expressed as The third pose corresponding to the third joint angle combination is determined by the following formula: Wherein, f T is a pose determination function.

[0076] After the third joint angle combination is determined, the forward solution matrix corresponding to the third joint angle combination, i.e., the third pose corresponding to the third joint angle combination, is determined. Wherein, the master hand controls the movement of each joint through the forward solution matrix.

[0077] It can be understood that the third pose is the superposition result of the first pose and the first pose offset.

[0078] S150, determine the pose difference between the third pose and the interpolation pose, and determine whether the third pose meets the second accuracy condition based on the pose difference.

[0079] After the third pose is determined, it is necessary to determine whether the third pose can be used, for this embodiment, after the pose difference is determined, the two norm of the pose difference is determined; if the two norm is greater than the set error threshold, it is judged that the current third pose does not meet the second accuracy condition, at this time the third pose cannot be used; if the two norm is less than or equal to the set error threshold, it is judged that the current third pose meets the second accuracy condition, at this time the third pose can be used, so the third joint angle combination corresponding to the third pose is taken as the target joint angle combination, that is, the inverse solution of the interpolation pose.

[0080] Specifically, the pose difference is determined by the following formula:

[0081]

[0082] The two norm of the pose difference is determined by the following formula:

[0083]

[0084] Wherein, e New[1] is the first element of e New , e New[2] is the second element of e New , e New[3] is the third element of e New .

[0085] S160, if no, taking the pose difference as the first pose offset, taking the third joint angle combination as the first joint angle combination, and returning to the step of determining the second joint angle combination corresponding to the first pose offset based on the first pose offset and the first joint angle combination until the third pose meets the second accuracy condition, and taking the third joint angle combination as the target joint angle combination.

[0086] In the case that the third pose does not meet the second accuracy condition, let e = e New , that is, taking the pose difference as the first pose offset, wherein e New is the pose difference, and e is the first pose offset; let q = q New , that is, taking the third joint angle combination as the first joint angle combination, wherein q New is the third joint angle combination, and q is the first joint angle combination; and then performing the above S130 until the third pose meets the second accuracy condition, and taking the third joint angle combination at this time as the target joint angle combination. Taking the first pose offset and the first joint angle combination as the iteration parameters, the third pose is updated until the third pose meets the second accuracy condition, which improves the accuracy of the third pose, thereby improving the accuracy of the determination of the target joint angle combination, that is, the accuracy of the determination of the first pose inverse solution.

[0087] After the target joint angle combination is determined, the movement of each joint of the robot is controlled according to the target joint combination; the actual angles of each joint corresponding to the movement results of each joint are determined to obtain the actual angles of each joint; and the actual angle combination is determined according to the actual angles of each joint, and the pose corresponding to the actual angle combination is taken as a new first pose, and the method returns to S110, so as to realize the technical effect that the target pose is approached through one or more times of interpolation.

[0088] The technical scheme provided by the embodiment of the application is based on the quaternion spherical surface pose interpolation method, the interpolation pose is determined according to the first pose, the target pose, the master hand movement direction and the slave arm movement direction, the rationality of the interpolation pose is improved, and the speed of approaching the target pose is improved; the first pose offset is updated through the pose difference between the third pose and the interpolation pose, the first joint angle combination is updated through the third joint angle combination, the third pose is updated until the third pose meets the second accuracy condition by taking the first pose offset and the first joint angle combination as the iteration parameters, the accuracy of the third pose is improved, and the accuracy of the determination of the target joint angle combination, that is, the accuracy of the determination of the first pose inverse solution, is improved.

[0089] Figure 2 Another flowchart of the robot master hand joint angle determination method provided by the embodiment of the application is provided, and the embodiment is used for refining the step of determining the inverse solution corresponding to the first pose offset. As shown in Figure 2 , the method comprises:

[0090] S210, if the first pose of the master hand does not meet the first accuracy condition, determining an interpolation pose based on a quaternion spherical interpolation method according to the first pose, a target pose, a master hand movement direction, and a slave arm movement direction.

[0091] S220, taking an offset of the interpolation pose from the first pose as a first pose offset, and taking a combination of joint angles corresponding to the first pose as a first joint angle combination, wherein a joint is used to connect two adjacent axes.

[0092] S230, determining a second joint angle combination corresponding to the first pose offset based on a damped least square method, the first pose offset, and the first joint angle combination.

[0093] The damped least square method is used to ensure that the master hand needs to overcome the minimum damping force in the process of completing the first pose offset.

[0094] In one embodiment, the second joint angle combination corresponding to the first pose offset is determined by the following steps, comprising:

[0095] Step a1, determining a Jacobian matrix of the robot and a transpose of the Jacobian matrix according to the first joint angle combination.

[0096] The prior art is used to determine a Jacobian matrix J corresponding to the first joint angle combination, and a transpose J T .

[0097] Step a2, determining a pseudo-inverse matrix of the Jacobian based on a damped least square method, the Jacobian matrix, and the transpose of the Jacobian matrix.

[0098] Specifically, the pseudo-inverse matrix of the Jacobian is determined by the following formula:

[0099] J + =J T *(J*J T +λ) -1

[0100] Wherein, λ is a damping factor, which is a non-zero constant, J + is the pseudo-inverse matrix of the Jacobian.

[0101] Step a3, determining a second joint angle combination including a joint differential motion angle based on the redundancy constraint control data, the first pose offset, and the pseudo-inverse matrix of the Jacobian.

[0102] In one embodiment, a differential entropy of the first joint angle combination is determined, and a gradient of the differential entropy is taken as the redundant constraint control data; a second joint angle combination including each joint differential motion angle is determined based on the redundant constraint control data, the first pose offset and the pseudo-inverse Jacobian matrix. Optionally, this embodiment can be implemented by the following formula:

[0103]

[0104] wherein e is the first pose offset, J + is the pseudo-inverse Jacobian matrix, J is the Jacobian matrix, H(q) represents the differential entropy of q, q is the first joint angle combination, ▽ is the gradient operator, ▽H(q) is the redundant constraint control data, K is a set constant, and I is a unit matrix with the same dimension as J + . Since the master robot in this embodiment is a 7-DOF redundant robot, in order to make its motion smoother, the redundant constraint control data is added so that the inverse solution determined directly satisfies the redundant constraint, without the need to add additional control methods.

[0105] After the second joint angle combination is determined, the second joint angle combination is taken as the inverse solution corresponding to the first pose offset. Compared with directly determining the inverse solution of the interpolated pose, this embodiment sets the motion control constraint strategy for the first pose offset on the basis of the first pose, which can improve the pertinence of the motion control constraint, thereby improving the accuracy of the inverse solution of the first pose offset, and the accuracy of the target joint angle combination (the inverse solution corresponding to the interpolated pose) determined based on the first pose offset and the first joint angle combination.

[0106] S240, determining a third joint angle combination and a third pose corresponding to the third joint angle combination according to the second joint angle combination and the first joint angle combination.

[0107] S250, determining a pose difference between the third pose and the interpolated pose, and determining whether the third pose meets a second accuracy condition based on the pose difference.

[0108] S260, if not, taking the pose difference as the first pose offset, taking the third joint angle combination as the first angle combination, and returning to the step of determining the second joint angle combination corresponding to the first pose offset based on the damped least squares method, the first pose offset and the first joint angle combination, until the third pose meets the second accuracy condition, and taking the third joint angle combination as the target joint angle combination.

[0109] The embodiment of the present application combines the damping least square method and the redundancy constraint control strategy to determine the inverse solution corresponding to the first pose offset, so that the inverse solution of the interpolation pose determined based on the inverse solution corresponding to the first pose offset directly meets the redundancy constraint, without the need to increase an additional control method, thereby ensuring the accuracy of the inverse solution of the interpolation pose and improving the determination speed of the inverse solution of the interpolation pose.

[0110] Figure 3 A structural schematic diagram of a robot master hand joint angle determination device is provided in the embodiment of the present application. As shown in the figure, Figure 3 The device comprises:

[0111] An interpolation pose module 31 is configured to, if the first pose of the master hand does not meet the first accuracy condition, determine an interpolation pose based on a quaternion spherical pose interpolation method according to the first pose, the target pose, the master hand movement direction and the slave arm movement direction.

[0112] A pose offset module 32 is configured to take the offset of the interpolation pose relative to the first pose as a first pose offset, and take the combination of the joint angles corresponding to the first pose as a first joint angle combination, wherein the joint is used to connect two adjacent axes.

[0113] An inverse solution module 33 is configured to determine a second joint angle combination corresponding to the first pose offset based on the first pose offset and the first joint angle combination.

[0114] A third pose module 34 is configured to determine a third joint angle combination and a third pose corresponding to the third joint angle combination according to the second joint angle combination and the first joint angle combination.

[0115] A pose difference module 35 is configured to determine the pose difference between the third pose and the interpolation pose, and determine whether the third pose meets a second accuracy condition based on the pose difference.

[0116] An iteration module 36 is configured to, if no, take the pose difference as the first pose offset, take the third joint angle combination as the first angle combination, and return to the step of determining the second joint angle combination corresponding to the first pose offset based on the first pose offset and the first joint angle combination, until the third pose meets the second accuracy condition, and take the third joint angle combination as a target joint angle combination.

[0117] In one embodiment, the interpolation pose module 31 is specifically configured to:

[0118] if the master hand motion direction is same as the slave arm motion direction, determining an interpolation pose based on a target pose offset between the target pose and the first pose and a quaternion spherical pose interpolation method along a direction from the first pose to the target pose;

[0119] if the master hand motion direction is opposite to the slave arm motion direction, determining an interpolation pose based on a set motion step length of the slave arm and a quaternion spherical pose interpolation method along a direction from the first pose to the target pose.

[0120] In one embodiment, the inverse solution module 33 is configured to:

[0121] determining a second joint angle combination corresponding to the first pose offset based on a damped least square method, the first pose offset and the first joint angle combination.

[0122] In one embodiment, the inverse solution module 33 comprises:

[0123] a Jacobian matrix unit configured to determine a Jacobian matrix of the robot and a transpose of the Jacobian matrix according to the first joint angle combination;

[0124] a pseudo-inverse matrix unit configured to determine a pseudo-inverse matrix of the Jacobian based on a damped least square method, the Jacobian matrix and the transpose of the Jacobian matrix;

[0125] a joint angle combination unit configured to determine a second joint angle combination comprising joint motion angles based on the pseudo-inverse matrix of the Jacobian, the first pose offset and redundancy constraint control data.

[0126] In one embodiment, the joint angle combination unit is specifically configured to:

[0127] determining a differential entropy of the first joint angle combination and a gradient of the differential entropy;

[0128] determining a second joint angle combination comprising joint differential motion angles based on the gradient of the differential entropy, the first pose offset and the pseudo-inverse matrix of the Jacobian.

[0129] In one embodiment, the joint angle combination unit is specifically configured to:

[0130]

[0131] wherein e is a first pose offset, J + is a pseudo-inverse matrix of the Jacobian, J is a Jacobian matrix, H(q) represents a differential entropy of q, q is a first joint angle combination, ▽ is a gradient operator, ▽H(q) is redundancy constraint control data, K is a set constant, and I is a unit matrix. +J is an identity matrix with the same dimension.

[0132] In one embodiment, the pose difference module 35 is specifically used for:

[0133] Determine the pose difference between the third pose and the interpolated pose;

[0134] Determine the L2 norm of the pose difference;

[0135] If the L2 norm is less than or equal to the set pose difference threshold, it means that the third pose meets the second precision condition;

[0136] If the L2 norm is greater than the set pose difference threshold, it means that the third pose does not meet the second precision condition.

[0137] The technical solution of this invention is based on the quaternion spherical pose interpolation method. It determines the interpolated pose according to the first pose, the target pose, the main hand movement direction, and the slave arm movement direction, improving the rationality of the interpolated pose and the speed of approximating the target pose. The first pose offset is updated by the pose difference between the third pose and the interpolated pose, and the first joint angle combination is updated by the third joint angle combination. Using the first pose offset and the first joint angle combination as iterative parameters, the third pose is updated until it meets the second accuracy condition, improving the accuracy of the third pose. This, in turn, improves the accuracy of determining the target joint angle combination, i.e., the accuracy of determining the inverse kinematics of the first pose.

[0138] Figure 4 A schematic diagram of the structure of a surgical robot 10 that can be used to implement embodiments of the present invention is shown. Figure 4 As shown, the surgical robot 10 includes a master arm 101, slave arms, and at least one processor 11 connecting the master arm 101 and the slave arm 102, as well as 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 can also store various programs and data required for the operation of the surgical robot 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.

[0139] A plurality of components in the surgical robot 10 are connected to the I / O interface 15, including: an input unit 16, such as a keyboard, a mouse, and the like; an output unit 17, such as various types of displays, speakers, and the like; a storage unit 18, such as a magnetic disk, an optical disk, and the like; and a communication unit 19, such as a network card, a modem, a wireless communication transceiver, and the like. The communication unit 19 allows the surgical robot 10 to exchange information / data with other devices through a computer network, such as the Internet, and / or various telecommunication networks.

[0140] The processor 11 can be various general and / or special purpose processing components having processing and computing capabilities. Some examples of the processor 11 include, but are not limited to, a central processing unit (CPU), a graphics processing unit (GPU), various specialized artificial intelligence (AI) computing chips, various processors running machine learning model algorithms, a digital signal processor (DSP), and any appropriate processor, controller, microcontroller, and the like. The processor 11 performs various methods and processes described above, such as the robot master hand joint angle determination method.

[0141] In some embodiments, the robot master hand joint angle determination method can be implemented as a computer program tangibly embodied in a computer readable storage medium, such as the storage unit 18. In some embodiments, part or all of the computer program can be loaded and / or installed onto the surgical robot 10 via the ROM 12 and / or the communication unit 19. When the computer program is loaded onto the RAM 13 and executed by the processor 11, one or more steps of the robot master hand joint angle determination method described above can be performed. Alternatively, in other embodiments, the processor 11 can be configured to perform the robot master hand joint angle determination method by any other appropriate means, such as by means of firmware.

[0142] Various implementations of the systems and techniques described above can be realized in digital electronic circuitry, integrated circuitry, a field programmable gate array (FPGA), an application specific integrated circuit (ASIC), a system on a chip (SOC), a programmable logic device (CPLD), computer hardware, firmware, software, and / or combinations thereof. These various implementations can include implementation in one or more computer programs that are executable and / or interpretable on a programmable system including at least one programmable processor, which can be special or general purpose, coupled to receive data and instructions from, and to transmit data and instructions to, a storage system, at least one input device, and at least one output device.

[0143] Computer programs for implementing the methods of the present application can be written in any combination of one or more programming languages. These computer programs can be provided to a processor of a general purpose computer, special purpose computer, or other programmable data processing apparatus to produce a machine, such that the computer program, when executed by the processor, implements the functions / acts specified in the flowcharts and / or block diagrams. The computer program can be executed entirely on a machine, partially on a machine, partially on a machine as a stand-alone software package, partially on a machine and partially on a remote machine, or entirely on a remote machine or server.

[0144] In the context of the present application, a computer-readable storage medium can be a tangible medium that can contain or store computer programs for use by or in connection with an instruction execution system, apparatus, or device. Computer-readable storage media can include, but are not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatus, or devices, or any suitable combination of the foregoing. Alternatively, a computer-readable storage medium can be a machine-readable signal medium. More specific examples of a machine-readable storage medium will include one or more lines of electrical connections, portable computer disks, hard disk drives, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or Flash memory), optical fibers, portable compact disc read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination of the foregoing.

[0145] To provide for interaction with a user, the systems and techniques described here can be implemented on a surgical robot having a display device (e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor) for displaying information to the user and a keyboard and a pointing device (e.g., a mouse or a trackball) by which the user can provide input to the surgical robot. Other kinds of devices can be used to provide for interaction with a user as well; 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 acoustic, speech, or tactile input.

[0146] The systems and techniques described herein can be implemented in a computing system that includes a back end component, e.g., as a data server, or that includes a middleware component, e.g., an application server, or that includes a front end component, e.g., a user computer having a graphical user interface or a Web browser through which a user can interact with an implementation of the systems and techniques described herein, or any combination of such back end, middleware, or front end components. The components of the system can be interconnected by any form or medium of digital data communication, e.g., a communication network. Examples of communication networks include a local area network (LAN), a wide area network (WAN), a blockchain network, and the Internet.

[0147] The computing system can include clients and servers. A client and server are generally remote from each other and typically interact through a communication network. The relationship of client and server arises by virtue of computer programs running on the respective computers and having a client-server relationship to each other. A server can be a cloud server, also known as a cloud computing server or cloud host, which is a host product in the cloud computing service system, to solve the defects of large management difficulty and weak business scalability in traditional physical host and VPS service.

[0148] It should be understood that the various forms of flow shown above can be re-ordered, added to, or deleted from without departing from the scope of the present disclosure. For example, the steps recited in the present disclosure can be performed in parallel, in series, or in a different order, and the present disclosure is not limited in this regard.

[0149] The specific embodiments described above are not intended to limit the scope of the present disclosure. Those skilled in the art will understand that various modifications, combinations, sub-combinations, and alternatives can be made to the specific embodiments without departing from the spirit and principles of the present disclosure. Any further modifications, equivalents, and / or alternatives come within the scope of the present disclosure as recited by the claims.

Claims

1. A robot master hand joint angle determination method characterized by, A method for controlling a seven-axis robot, the seven-axis robot comprising a master hand and a slave arm, the master hand comprising seven axes, the method comprising: if a first pose of the master hand does not meet a first accuracy condition, determining an interpolated pose based on a quaternion spherical pose interpolation method according to the first pose, a target pose, a master hand movement direction, and a slave arm movement direction; determining a second joint angle combination corresponding to the first pose offset based on the first pose offset and the first joint angle combination; determining a third joint angle combination according to the second joint angle combination and the first joint angle combination, and a third pose corresponding to the third joint angle combination; determining a pose difference between the third pose and the interpolated pose, and determining whether the third pose meets a second accuracy condition based on the pose difference; if not, taking the pose difference as the first pose offset, taking the third joint angle combination as the first angle combination, and returning to the step of determining the second joint angle combination corresponding to the first pose offset based on the first pose offset and the first joint angle combination, until the third pose meets the second accuracy condition, and taking the third joint angle combination as a target joint angle combination. The method for controlling a seven-axis robot, the seven-axis robot comprising a master hand and a slave arm, the master hand comprising seven axes, the method comprising:

2. The method of claim 1, wherein, if the master hand movement direction and the slave arm movement direction are in the same direction, determining an interpolated pose based on a target pose offset between the target pose and the first pose and the quaternion spherical pose interpolation method along a direction from the first pose to the target pose; if the master hand movement direction and the slave arm movement direction are in different directions, determining an interpolated pose based on a set movement step of the slave arm and the quaternion spherical pose interpolation method along a direction from the first pose to the target pose. The method for controlling a seven-axis robot, the seven-axis robot comprising a master hand and a slave arm, the master hand comprising seven axes, the method comprising:

3. The method of claim 1, wherein, determining the second joint angle combination corresponding to the first pose offset based on a damped least square method, the first pose offset and the first joint angle combination. The method for controlling a seven-axis robot, the seven-axis robot comprising a master hand and a slave arm, the master hand comprising seven axes, the method comprising:

4. The method of claim 3, wherein, determining a Jacobian matrix of the robot and a transpose of the Jacobian matrix according to the first joint angle combination; determining a pseudo-inverse matrix of the Jacobian matrix based on a damped least square method, the Jacobian matrix and the transpose of the Jacobian matrix; determining a second joint angle combination comprising joint movement angles based on the pseudo-inverse matrix of the Jacobian matrix, the first pose offset and redundancy constraint control data. ​ 5. The method of claim 4, wherein, determining a second joint angle combination including joint motion angles based on the pseudo-inverse matrix, the first pose offset, and the redundant constraint control data, including: determining a differential entropy of the first joint angle combination, and a gradient of the differential entropy, taking the gradient of the differential entropy as the redundant constraint control data; determining a second joint angle combination including joint differential motion angles based on the redundant constraint control data, the first pose offset, and the pseudo-inverse matrix.

6. The method of claim 5, wherein, The second joint angle combination including each joint motion angle is determined based on the Jacobian pseudo-inverse matrix, the first pose offset and the redundant constraint control data, comprising wherein, is the second joint angle combination, e is the first pose offset, J + is the Jacobian pseudo-inverse matrix, J is the Jacobian matrix, H(q) represents the differential entropy of q, q is the first joint angle combination, and ▽ is the gradient operator, is the redundant constraint control data, K is a set constant, and I is a unit matrix with the same dimension as J + J.

7. The method of claim 1, wherein, determining whether the third pose meets the accuracy condition by the following steps: determining a pose difference between the third pose and the interpolation pose; determining a two-norm of the pose difference; if the two-norm is less than or equal to a set pose difference threshold, it indicates that the third pose meets the second accuracy condition; if the two-norm is greater than a set pose difference threshold, it indicates that the third pose does not meet the second accuracy condition.

8. A device for determining the joint angle of a robot's main hand, characterized in that, including: an interpolation pose module configured to, if the first pose of the master hand does not meet the first accuracy condition, determine an interpolation pose based on a quaternion spherical pose interpolation method according to the first pose, a target pose, a master hand motion direction, and a slave arm motion direction; a pose offset module configured to take an offset of the interpolation pose relative to the first pose as a first pose offset, and take a combination of joint angles corresponding to the first pose as a first joint angle combination, wherein a joint is used to connect two adjacent axes; an inverse solution module configured to determine a second joint angle combination corresponding to the first pose offset based on the first pose offset and the first joint angle combination; a third pose module configured to determine a third joint angle combination according to the second joint angle combination and the first joint angle combination, and a third pose corresponding to the third joint angle combination; a pose difference module configured to determine a pose difference between the third pose and the interpolation pose, and determine whether the third pose meets a second accuracy condition based on the pose difference; an iteration module configured to, if not, take the pose difference as the first pose offset, take the third joint angle combination as a first angle combination, and return to the step of determining the second joint angle combination corresponding to the first pose offset based on the first pose offset and the first joint angle combination, until the third pose meets the second accuracy condition, and take the third joint angle combination as a target joint angle combination.

9. A surgical robot, characterized by The surgical robot includes: a master hand and a slave arm; at least one processor connected between the master hand and the slave arm; and a memory in communication connection with the at least one processor; wherein the memory stores a computer program executable by the at least one processor, and the computer program is executed by the at least one processor to enable the at least one processor to execute the robot master hand joint angle determination method of any one of claims 1-7.

10. A computer-readable storage medium, characterized in that, The computer readable storage medium stores computer instructions for enabling the processor to execute the robot master hand joint angle determination method of any one of claims 1-7 when executed by the processor.

Images