A robot master manipulator control system and robot

Abstract

The embodiment of the application discloses a robot master operating hand control system and a robot. The system comprises a master controller and a slave controller integrated on the robot, the master controller corresponds to a master operating hand in the robot, the slave controller corresponds to a slave end mechanical arm in the robot, and a target instrument is installed on the slave end mechanical arm. The slave controller is used for processing a target object at an instrument end of the target instrument, and in the case that an applied contact force on the target object at the instrument end is greater than a safe contact force that the target object can bear, a slave arm impedance torque generated by an instrument joint of the instrument end is determined, and the slave arm impedance torque is sent to the master controller. The master controller is used for determining a joint impedance torque applied on a master hand joint of the master operating hand according to the slave arm impedance torque, and controlling the master operating hand to move according to the joint impedance torque. The technical scheme of the embodiment of the application can guide an operator of the master operating hand to safely operate.

Description

TECHNICAL FIELD

[0001] Embodiments of the present application relate to the field of robots, and in particular to a robot master manipulator control system and a robot. BACKGROUND

[0002] Laparoscopic surgery robots are the product of the combination of modern medical technology and robot technology. With the continuous development of laparoscopic surgery robots, their application in laparoscopic surgery is becoming more and more common.

[0003] The laparoscopic surgery robot includes a master manipulator and a slave manipulator, and a surgeon can control the movement of the slave manipulator by operating the master manipulator, so as to perform surgery by means of a surgical instrument installed on the slave manipulator.

[0004] At present, in the case where the surgical instrument is in contact with the human tissue, the surgeon cannot accurately perceive the specific contact situation, which easily leads to damage to the human tissue and needs to be solved urgently. SUMMARY

[0005] Embodiments of the present application provide a robot master manipulator control system and a robot to guide the operator of the master manipulator in the robot to operate safely.

[0006] According to an aspect of the present application, a robot master manipulator control system can include: a master controller and a slave controller integrated on a robot, the master controller corresponding to a master manipulator in the robot, and the slave controller corresponding to a slave manipulator in the robot, and a target instrument being installed on the slave manipulator:

[0007] The slave controller is configured to determine a slave impedance torque generated by an instrument joint of an instrument end of the target instrument in the case where an applied contact force exerted by the instrument end on a target object is greater than a safe contact force that the target object can withstand, and send the slave impedance torque to the master controller.

[0008] The master controller is configured to determine a joint impedance torque applied to a master joint of the master manipulator according to the slave impedance torque, and control the movement of the master manipulator according to the joint impedance torque.

[0009] Optionally, the slave controller is further configured to:

[0010] determine an actual output current value of an instrument motor of the instrument joint in a current period, a previous output current value of the instrument motor in a previous period of the current period, and a theoretical output current value of the instrument motor in an idle state.

[0011] determine the change of the contact force according to the absolute value of the first difference between the actual output current value and the theoretical output current value, and a second difference between the actual output current value and a previous output current value;

[0012] from the controller, specifically for:

[0013] in the case where the change indicates that the contact force is increasing, determine the slave arm impedance torque generated by the instrument joint at the instrument end according to the first difference and the second difference, and in the case where the change indicates that the contact force is decreasing, take zero as the slave arm impedance torque;

[0014] send the slave arm impedance torque to the master controller.

[0015] On this basis, an optional slave controller is further used for:

[0016] for the slave arm impedance torque to be determined generated by the instrument joint at the instrument end, determine the impedance torque direction of the slave arm impedance torque according to the forward kinematics of the robot arm;

[0017] from the controller, specifically for:

[0018] in the case where the change indicates that the contact force is increasing, determine the impedance torque of the slave arm impedance torque according to the first difference and the second difference;

[0019] determine the slave arm impedance torque according to the impedance torque direction and the impedance torque size.

[0020] Another optional slave controller is specifically used for:

[0021] in the case where the instrument motor is in an idle state, obtain the angle state of the instrument joint according to the data recorded in the motor encoder of the instrument motor;

[0022] obtain the theoretical output current value of the instrument motor according to the angle state.

[0023] Optionally, the slave controller is further used for:

[0024] for the instrument motor of the instrument joint, determine the absolute value of the first difference between the actual output current value of the instrument motor and the theoretical output current value of the instrument motor in the idle state;

[0025] obtain a contact threshold preset for a safe contact force, and in the case where the absolute value is greater than the contact threshold, determine that the contact force is greater than the safe contact force.

[0026] Optionally, the slave controller is further used for:

[0027] determine the contact force, and send the contact force to the master controller;

[0028] The master controller is also configured to:

[0029] determine a perceived feedback force applied on the master manipulator according to the applied contact force, wherein a contact force direction of the applied contact force is the same as a feedback force direction of the perceived feedback force;

[0030] determine a joint perceived torque applied on the master joint according to the perceived feedback force;

[0031] The master controller is specifically configured to:

[0032] control the master manipulator movement according to the joint perceived torque and the joint impedance torque.

[0033] On this basis, an optional slave controller is specifically configured to:

[0034] determine a contact force magnitude and a contact force direction of the applied contact force for the applied contact force to be determined, and determine the applied contact force according to the contact force magnitude and the contact force direction;

[0035] send the applied contact force to the master controller.

[0036] On this basis, an optional slave controller is specifically configured to:

[0037] determine an absolute value of a first difference between an actual output current value of an instrument motor of the instrument joint and a theoretical output current value of the instrument motor in an idle state;

[0038] obtain a preset motor current-torque coefficient for the instrument joint, and determine a contact force magnitude of the applied contact force according to the motor current-torque coefficient and the absolute value.

[0039] Another optional slave controller is specifically configured to:

[0040] determine an actual position of an instrument tip, and calculate a theoretical position of the instrument tip according to a forward solution of the master manipulator movement and a master-slave operation ratio;

[0041] determine a contact force direction of the applied contact force according to a position difference between the actual position and the theoretical position.

[0042] Another optional master controller is specifically configured to:

[0043] control the master manipulator movement according to a position component and a perceived attitude component in the joint perceived torque, and an impedance attitude component in the joint impedance torque.

[0044] On the basis of any of the above technical solutions, the robot is a surgical robot, and the target instrument is a surgical instrument; and / or,

[0045] The joint type of the instrument joint includes at least one of a swing joint, a pitch joint and a yaw joint, and the joint type of the master joint corresponds to the joint type of the instrument joint.

[0046] According to another aspect of the present application, a robot can include a master operating hand, a slave end mechanical arm and a robot master operating hand control system according to any of the embodiments of the present application.

[0047] The robot master operating hand control system according to the embodiments of the present application can include a master controller and a slave controller integrated on the robot, the master controller corresponding to the master operating hand in the robot and the slave controller corresponding to the slave end mechanical arm in the robot, and a target instrument being installed on the slave end mechanical arm. On this basis, when the applied contact force on the target object at the instrument end of the target instrument is greater than the safe contact force that the target object can bear, the slave arm impedance torque that needs to be generated on the instrument joint at the instrument end is determined by using the slave controller, and the slave arm impedance torque can make the instrument joint move towards the safe direction; further, the joint impedance torque applied on the master joint is determined by using the master controller according to the slave arm impedance torque sent by the slave controller, so that the master operating hand can be controlled to move according to the joint impedance torque, so as to prompt the operator to operate the master operating hand. The above technical solution controls the movement of the master operating hand to guide the operator to operate the master operating hand towards the safe direction, that is, to perform the master-slave operation towards the safe direction, so that the operator can also realize safe operation even if he cannot accurately perceive the specific contact between the instrument end and the target object.

[0048] It should be understood that the description in this section is not intended to identify key or essential features of 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

[0049] 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.

[0050] Figure 1 is a structural block diagram of a robot master operating hand control system according to an embodiment of the present application;

[0051] Figure 2 is a schematic diagram of part of the mechanism associated with the slave end mechanical arm in a robot master operating hand control system according to an embodiment of the present application;

[0052] Figure 3is a schematic diagram of an optional example of a robot master manipulator control system according to an embodiment of the present application;

[0053] Figure 4 is a structural block diagram of another robot master manipulator control system according to an embodiment of the present application. DETAILED DESCRIPTION

[0054] 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 in combination with the drawings in the embodiment of the present application. Obviously, the described embodiment is only a part of the embodiment of the present application, not all. Based on the embodiment 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.

[0055] 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. The case of "target", "original" and the like is similar, which will not be repeated here. 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 limit 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.

[0056] Figure 1 is a structural block diagram of a robot master manipulator control system according to an embodiment of the present application. The present embodiment can be applied to control the movement of the master manipulator in the robot to guide the safe operation of the operator of the master manipulator, especially to control the movement of the master manipulator in the surgical robot to guide the safe operation of the doctor, and the surgical robot can be a laparoscopic surgical robot in particular.

[0057] Referring to Figure 1 , the robot master manipulator control system described in the embodiment of the present application comprises a master controller 11 and a slave controller 10 integrated on a robot, the master controller 11 corresponds to a master manipulator in the robot, and the slave controller 10 corresponds to a slave end mechanical arm in the robot, and a target instrument is installed on the slave end mechanical arm:

[0058] wherein the slave controller 10 is configured to determine a slave joint torque of an instrument joint of the instrument tip of the target instrument in response to the target object being handled by the instrument tip of the target instrument and the applied contact force being greater than the safe contact force, and send the slave joint torque to the master controller 11;

[0059] The master controller 11 is configured to determine a joint impedance torque of a master hand joint of the master hand in response to the slave joint torque, and control the master hand to move in response to the joint impedance torque.

[0060] The robot comprises the master controller 11, the slave controller 10, the master hand and the slave end mechanical arm, the master controller 11 can be understood as a controller configured to process data related to the master hand, and the slave controller 10 can be understood as a controller configured to process data related to the slave end mechanical arm, and the target instrument is installed on the slave end mechanical arm. In combination with the application scenarios that can be involved in the embodiments of the present application, the robot can be a surgical robot, and the target instrument can be a surgical instrument; of course, the robot can also be an industrial robot, and the target instrument can be an industrial instrument used in an industrial scenario; and the like, which are not limited herein.

[0061] The target object can be understood as an object that can be handled by the target instrument, for example, human tissue that can be handled by a surgical instrument or industrial articles that can be handled by an industrial instrument, etc. The instrument tip can be understood as a part of the target instrument that contacts the target object and thus handles the target object. The instrument joint can be understood as a joint on the instrument tip, and in combination with the application scenarios that can be involved in the embodiments of the present application, the joint type of the instrument joint can comprise at least one of a swing joint, a pitch joint and a yaw joint. On this basis, it can be understood that the joint type of the master hand joint corresponds to the joint type of the instrument joint, so as to cooperate with each other.

[0062] The applied contact force can be understood as a contact force applied by the instrument tip to the target object when the instrument tip contacts the target object. The safe contact force can be understood as the maximum contact force that can be borne by the target object, in other words, when the contact force applied to the target object is greater than the safe contact force, the target object will be damaged. Therefore, in the case that the applied contact force is greater than the safe contact force, a safety control strategy needs to be triggered to guide the operator to operate the master hand in a certain way, so as to avoid damaging the target object, and the specific process is as follows:

[0063] The slave arm impedance torque to be generated on the instrument joint is determined by the slave controller 10, which can be understood as a torque for rotating the instrument joint so as to move the instrument joint towards the safe direction (i.e. the direction of reducing the applied contact force of the instrument end on the target object), thereby ensuring the safety of the target object. Then, the slave arm impedance torque is sent to the master controller 11 by the slave controller 10, so as to determine the joint impedance torque to be applied on the master hand joint of the master manipulator on the master controller 11.

[0064] The joint impedance torque to be applied on the master hand joint of the master manipulator is determined by the master controller 11 according to the slave arm impedance torque. In combination with the application scenarios that the embodiments of the present application can involve, the master hand impedance torque applied on the master manipulator can be determined according to the slave arm impedance torque, and the master hand impedance torque is converted into the corresponding joint impedance torque of the master hand joint. Further, the master manipulator is controlled based on the joint impedance torque, so as to prompt the operator to operate the master manipulator in a certain manner, for example, to prompt the operator to move in a certain direction and to move to a certain position in the direction, etc. When the operator operates the master manipulator according to the prompt, the instrument joint (i.e. the instrument end) can move towards the safe direction under the master-slave operation mechanism in the robot, thereby avoiding the damage to the target object.

[0065] The robot master manipulator control system according to the embodiments of the present application can include a master controller and a slave controller integrated on the robot, the master controller corresponds to the master manipulator in the robot, and the slave controller corresponds to the slave end manipulator in the robot, and the target instrument is installed on the slave end manipulator. On this basis, when the applied contact force of the instrument end of the target instrument on the target object is greater than the safe contact force that the target object can bear, the slave arm impedance torque to be generated on the instrument joint of the instrument end is determined by the slave controller, and the slave arm impedance torque can move the instrument joint towards the safe direction. Further, the joint impedance torque to be applied on the master hand joint is determined by the master controller according to the slave arm impedance torque sent by the slave controller, so as to control the movement of the master manipulator according to the joint impedance torque, thereby prompting the operator to operate the master manipulator in a certain manner. The above technical solution controls the movement of the master manipulator, thereby guiding the operator to operate the master manipulator towards the safe direction, i.e. to perform the master-slave operation towards the safe direction, so that the operator can also perform the safe operation even if the operator cannot accurately perceive the specific contact between the instrument end and the target object.

[0066] Before introducing the following technical solutions, the application scenarios that the embodiments of the present application can involve are exemplarily described. Exemplarily, referring to Figure 2, the content in the dashed box on the left side of the figure is a top view of the content in the dashed box on the top of the figure, and the instrument end 2 is circled by a dashed line on the right side of the figure. Here, taking the instrument joints including the instrument roll joint, the instrument pitch joint and the instrument yaw joint as an example, the wire transmission disc 1 is fixedly connected with an instrument roll motor for controlling the instrument roll joint, an instrument pitch motor for controlling the instrument pitch joint and an instrument yaw motor for controlling the instrument yaw joint, and a target instrument is installed on the wire transmission disc 1, so that the rotation of the instrument roll motor, the instrument pitch motor and the instrument yaw motor can realize the movement of each instrument joint on the instrument end 2 by means of the wire transmission mode, thereby completing the action required for processing the target object.

[0067] On this basis, the action implementation corresponding to the mechanism implementation shown in Figure 2 is as follows: when each instrument joint is not subjected to contact force, i.e. in the no-load state, the instrument roll motor code disc value θ 编1 , the instrument roll joint angle value θ 回转 and the instrument roll motor theoretical output current value I 回转1 correspond to each other; the instrument pitch motor code disc value θ 编2 , the instrument pitch joint angle value θ 俯仰 and the instrument pitch motor theoretical output current value I 俯仰1 correspond to each other; and the instrument yaw motor code disc value θ 编3 , the instrument yaw joint angle value θ 偏摆 and the instrument yaw motor theoretical output current value I 偏摆1 correspond to each other.

[0068] Referring to Figure 3 , in the master-slave operation, the instrument motor shaft rotation angle can be recorded by the encoder module, the angle state of the instrument roll joint, the instrument pitch joint and the instrument yaw joint corresponding to the master-slave operation can be obtained, and then the instrument roll motor code disc value θ 编1 , the instrument roll joint angle value θ 回转 and the instrument roll motor actual output current value I 回转2 can be obtained; the instrument pitch motor code disc value θ 编2 , the instrument pitch joint angle value θ 俯仰 and the instrument pitch motor output current value I 俯仰2 can be obtained; and the instrument yaw motor code disc value θ 编3 , the instrument yaw joint angle value θ 偏摆 and the instrument yaw motor output current value I 偏摆2 can be obtained.

[0069] On this basis, according to the action implementation, the master-slave operation can collect the instrument roll motor code disc value θ 编1 , the instrument pitch motor code disc value θ 编2θ 编3 θ 回转 θ 俯仰 θ 偏摆 θ 回转1 θ 俯仰1 θ 偏摆1 θ 回转2 θ 俯仰2 θ 偏摆2 .

[0070] On this basis, if the three instrument joints of the instrument end have no external physical contact, at this time:

[0071]

[0072] If the three instrument joints of the instrument end have external physical contact, and the applied contact force applied by the instrument end on the target object is greater than the safe contact force that the target object can bear, at this time:

[0073]

[0074] At this time, the safety control strategy needs to be triggered, that is, the operator is guided to operate safely.

[0075] In other words, an optional technical solution, from the controller, further for:

[0076] For the instrument motor of the instrument joint, determining the absolute value of the first difference between the actual output current value of the instrument motor and the theoretical output current value of the instrument motor in the no-load state;

[0077] Obtaining a contact threshold value preset for the safe contact force, and determining that the applied contact force is greater than the safe contact force in the case that the absolute value is greater than the contact threshold value.

[0078] The above technical solution compares the actual output current value with the theoretical output current value, and accurately determines the size relationship between the applied contact force and the safe contact force.

[0079] Another optional technical solution, from the controller, further for:

[0080] The actual output current value of the instrument motor in the current period, the previous output current value of the instrument motor in the previous period of the current period, and the theoretical output current value of the instrument motor in the no-load state are determined, and the variation of the contact force is determined according to the absolute value of the first difference between the actual output current value and the theoretical output current value, and the second difference between the actual output current value and the previous output current value.

[0081] The controller is specifically configured to:

[0082] When the variation indicates that the contact force is increasing, the slave arm impedance torque generated by the instrument joint at the instrument end is determined according to the first difference and the second difference, and when the variation indicates that the contact force is decreasing, zero is taken as the slave arm impedance torque; the slave arm impedance torque is sent to the master controller.

[0083] The current value actually output by the instrument motor is refreshed in real time. On this basis, the actual output current value can be understood as the current value actually output by the instrument motor in the current period, the previous output current value can be understood as the current value actually output by the instrument motor in the previous period of the current period, and the theoretical output current value can be understood as the current value actually output by the instrument motor in the no-load state, i.e. the current value that should be theoretically output.

[0084] In actual application, in combination with the above examples, optionally, when the instrument motor is in the no-load state, the angle state of the instrument joint can be obtained according to the data recorded in the motor encoder of the instrument motor, and then the theoretical output current value can be obtained according to the angle state.

[0085] After the above output current values are determined, the variation of the contact force can be determined according to the absolute value of the first difference between the actual output current value and the theoretical output current value, and the second difference between the actual output current value and the previous output current value, which can reflect that the contact force is increasing, unchanged or decreasing from the previous period to the current period.

[0086] On this basis, further, if the variation indicates that the contact force is increasing, it indicates that the instrument end is moving towards the dangerous direction, and then the slave arm impedance torque can be determined according to the first difference and the second difference to guide the operator to control the master operating hand to move the instrument end towards the safe direction; if the variation indicates that the contact force is decreasing, it indicates that the instrument end is moving towards the safe direction (i.e. the direction away from the dangerous direction), and then zero can be taken as the slave arm impedance torque, i.e. the operator does not need to be guided at this time.

[0087] In actual application, for the case that the contact force increases, the impedance torque direction of the slave arm impedance torque can be determined according to the forward kinematics of the robot arm, which can be understood as the movement direction vector of the joint rotation axis of the instrument relative to the base coordinate system, and the impedance torque size of the slave arm impedance torque can be determined according to the first difference and the second difference; further, the slave arm impedance torque can be determined according to the impedance torque direction and the impedance torque size, and the common application of the impedance torque direction and the impedance torque size ensures the accurate determination of the slave arm impedance torque.

[0088] The above technical solution determines whether the operator needs to be guided to operate and how to guide the operator to operate by judging the change of the contact force of the contact force, thereby further ensuring the operation safety.

[0089] Figure 4 is another structural block diagram of a robot master operating hand control system provided by the embodiment of the application. The embodiment is optimized on the basis of the above technical solutions. In the embodiment, the slave controller is further used to: determine the contact force of the applied contact force and send the contact force of the applied contact force to the master controller; the master controller is further used to: determine the perception feedback force applied on the master operating hand according to the contact force of the applied contact force, wherein the contact force direction of the applied contact force is the same as the feedback force direction of the perception feedback force; determine the joint perception torque applied on the master hand joint according to the perception feedback force; and the master controller is specifically used to: control the movement of the master operating hand according to the joint perception torque and the joint impedance torque. Wherein, the explanations of the same or corresponding terms in the above embodiments are not repeated here.

[0090] Referring to Figure 4 The robot master operating hand control system described in the embodiment includes a master controller 21 and a slave controller 20 integrated on a robot, the master controller 21 corresponds to a master operating hand in the robot, the slave controller 20 corresponds to a slave end mechanical arm in the robot, and a target instrument is installed on the slave end mechanical arm:

[0091] The slave controller 20 is used to determine the applied contact force of the instrument end on the target object for the target object handled by the instrument end of the target instrument, and send the applied contact force to the master controller 21 when the applied contact force is greater than the safe contact force that the target object can bear;

[0092] The slave controller 20 is further used to determine the slave arm impedance torque generated by the instrument joint of the instrument end and send the slave arm impedance torque to the master controller 21;

[0093] The main controller 21 is configured to determine a perceived feedback force applied to the master manipulator according to the contact force, and determine a joint perceived torque applied to the joint of the master manipulator according to the perceived feedback force, wherein the contact force direction of the contact force is the same as the feedback force direction of the perceived feedback force.

[0094] The main controller 21 is further configured to determine a joint impedance torque applied to the joint of the master manipulator according to the impedance torque of the slave arm, and control the movement of the master manipulator according to the joint perceived torque and the joint impedance torque.

[0095] As described above, the operator cannot accurately perceive the specific contact between the instrument tip and the target object, and in order to further solve this technical problem, the main controller 21 can receive the determined contact force from the slave controller 20; in this way, the main controller 21 can determine a perceived feedback force applied to the master manipulator according to the contact force, and the contact force direction of the contact force is the same as the feedback force direction of the perceived feedback force, that is, the direction of the instrument tip relative to the lens is consistent with the direction of the master manipulator relative to the display; then, the main controller 21 converts the perceived feedback force into a joint perceived torque applied to the joint of the master manipulator, and then controls the movement of the master manipulator by combining the joint perceived torque and the joint impedance torque, so that the operator can perceive the contact force applied to the target object.

[0096] In actual application, when the two joint torques are combined to control the movement of the master manipulator, the joint perceived torque is the basis, the joint impedance torque is the advanced, and the joint impedance torque is mainly set to increase the posture component, so the master manipulator can be controlled according to the position component and the perceived posture component in the joint perceived torque, and the impedance posture component in the joint impedance torque.

[0097] The technical scheme of the embodiment of the application converts the contact force into a joint perceived torque applied to the joint of the master manipulator, and controls the movement of the master manipulator based on this, so that the operator can perceive the contact force applied to the target object, and further ensures the operation safety.

[0098] An optional technical scheme, the slave controller is specifically configured to:

[0099] For the contact force to be determined, determine the contact force size and the contact force direction of the contact force, and determine the contact force according to the contact force size and the contact force direction.

[0100] Send the contact force to the main controller.

[0101] The above technical scheme determines the contact force size and the contact force direction, and realizes accurate determination of the contact force.

[0102] On this basis, an optional slave controller is specifically used for:

[0103] For the instrument motor of the instrument joint, determining an absolute value of a first difference between an actual output current value of the instrument motor and a theoretical output current value of the instrument motor in an idle state;

[0104] Obtaining a preset motor current-torque coefficient for the instrument joint, and determining a contact force size of the contact force according to the motor current-torque coefficient and the absolute value.

[0105] Wherein, the instrument end is blocked by the target object, so that the actual output current value of the instrument motor is greater than the theoretical output current value, therefore, the absolute value of the first difference between the two can be combined with the motor current-torque coefficient to obtain the extra motor driving torque, and then the contact force size can be calculated according to the motor driving torque, so that the two output current values are used to accurately determine the contact force size.

[0106] Another optional slave controller is specifically used for:

[0107] Determining an actual position of the instrument end, and calculating a theoretical position of the instrument end according to the forward solution of the master operation hand and the master-slave operation ratio;

[0108] Determining a contact force direction of the contact force according to a position difference between the actual position and the theoretical position.

[0109] Wherein, the instrument end is blocked by the target object, so that there is a difference between the theoretical position and the actual position of the instrument end under the master-slave operation, therefore, the contact force direction can be determined according to the position difference between the two.

[0110] In combination with the application scenarios that the embodiments of the present application can involve, a robot is provided, which can include a master operation hand, a slave mechanical arm, and a robot master operation hand control system described in any of the above technical solutions. The robot can guide the operator of the master operation hand to operate safely.

[0111] In order to better understand the above-mentioned various technical solutions as a whole, the following will continue to be exemplarily described on the basis of the above examples. Exemplarily, continue to refer to Figure 3 , and especially refer to the content after triggering the safety control in Figure 3 . Here, taking an n-degree-of-freedom surgical robot as an example, the specific implementation process is as follows:

[0112] 1. The model of the contact force applied when the instrument end contacts with the human tissue:

[0113] According to the transformation matrix of each connecting rod Multiplying, the transformation matrix from the base coordinate system of the n-DOF slave end manipulator to the instrument joint is obtained

[0114]

[0115] where each θ is the joint angle of the corresponding instrument joint corresponding to the link.

[0116] The forward kinematics equation of the n-DOF slave end manipulator is obtained as:

[0117]

[0118] where n, o and a in each direction can constitute a rotation matrix, and p in each direction can constitute a translation matrix.

[0119] According to the above formula, the actual position of the instrument tip relative to the base coordinate system under master-slave operation, i.e., the actual position p x of the instrument tip relative to the x direction in the base coordinate system, the actual position p y of the instrument tip relative to the y direction in the base coordinate system, and the actual position p z of the instrument tip relative to the z direction in the base coordinate system, can be obtained.

[0120] According to the forward kinematics calculation of the master manipulator and considering the master-slave operation ratio, the theoretical position of the instrument tip relative to the x direction in the base coordinate system p x1 , the theoretical position of the instrument tip relative to the y direction in the base coordinate system p y1 , and the theoretical position of the instrument tip relative to the z direction in the base coordinate system p z1 .

[0121] Since the instrument tip contacts the human tissue, the instrument joint is driven by the instrument motor to move towards the theoretical position, resulting in that the actual output current value of the instrument motor is greater than the theoretical output current value, triggering the following condition:

[0122]

[0123] The actual position of the instrument tip relative to the base coordinate system under the master-slave operation intention is obtained, and the difference between the actual position and the theoretical position is the force direction of the instrument tip and the human tissue, i.e., the contact force direction. Here, the base coordinate system is taken as the reference coordinate system, and the force direction vector of the instrument tip of the n-DOF slave end manipulator relative to the base coordinate system is obtained as

[0124] If |I Δ回转 | > contact threshold value, the contact force between the instrument tip and the human tissue is:

[0125]

[0126] Among them, M 回转coe L is the motor current-torque coefficient. 回转 The radius of the wire drive disc of the instrument's rotary motor.

[0127] If |I Δ俯仰 |>Contact threshold, the magnitude of the contact force applied between the instrument tip and human tissue:

[0128]

[0129] Among them, M 俯仰coe L is the motor current-torque coefficient. 俯仰 The radius of the wire drive disc of the instrument's pitch motor.

[0130] If |I Δ偏摆 The contact threshold, which is the magnitude of the contact force applied between the instrument tip and human tissue:

[0131]

[0132] Among them, M 偏摆coe L is the motor current-torque coefficient. 偏摆 The radius of the wire drive disc of the instrument's yaw motor.

[0133] Using the above size model, the magnitude of the contact force generated when the instrument tip contacts human tissue relative to the base coordinate system can be obtained:

[0134]

[0135] Combining the directional model, the applied contact force in the base coordinate system is expressed as F = (F X ,F Y ,F Z ),in:

[0136]

[0137] 2. Instrument end contact torque model (i.e., follower arm impedance torque model):

[0138] When the following conditions are triggered:

[0139]

[0140] Based on the forward kinematics of the robotic arm, the direction of the rotational impedance torque in the base coordinate system can be obtained, that is, the motion direction vector of the machine's rotary joint axis relative to the base coordinate system is... Similarly, the direction vector of motion of the device's pitch joint axis relative to the base coordinate system can be obtained as follows: And the direction vector of motion of the instrument's yaw joint axis relative to the base coordinate system is

[0141] When the physician triggers the above condition, the movement continues in the dangerous direction, and

[0142] I Δ回转 ·I δ回转 > 0;

[0143] wherein I δ回转 is the instrument roll motor self-change amount, i.e., the actual output current value of the instrument roll motor in the current period - the actual output current value in the last period (i.e., the last output current value in the above).

[0144] I Δ俯仰 ·I δ俯仰 > 0;

[0145] wherein I δ俯仰 is the instrument pitch motor self-change amount, i.e., the actual output current value of the instrument pitch motor in the current period - the actual output current value in the last period (i.e., the last output current value in the above).

[0146] I Δ偏摆 ·I δ偏摆 > 0;

[0147] wherein I δ偏摆 is the instrument yaw motor self-change amount, i.e., the actual output current value of the instrument yaw motor in the current period - the actual output current value in the last period (i.e., the last output current value in the above).

[0148] Let:

[0149] K IΔ回转 = I Δ回转 ·I δ回转 ;

[0150] K IΔ俯仰 = I Δ俯仰 ·I δ俯仰 ;

[0151] K IΔ偏摆 = I Δ偏摆 ·I δ偏摆 ;

[0152] wherein when K and K , i.e., the applied contact force generated by the instrument roll joint, the instrument pitch joint and the instrument yaw joint contacting the human tissue increases, which indicates that each instrument joint is moving in the dangerous direction, then the model of the arm impedance torque is as follows:

[0153]

[0154] Wherein, each M represents the corresponding arm impedance torque of the instrument joint, K1 is the impedance torque coefficient.

[0155] When and , that is, the instrument rotary joint, instrument pitch joint and instrument yaw joint contact the human tissue and move in the safe direction, the model of the arm impedance torque is as follows:

[0156]

[0157] 3. Master hand sensing feedback model:

[0158] The contact force direction and contact force size of the contact force generated by the instrument end contacting the human tissue relative to the base coordinate system are applied to the master hand end of the master hand relative to the base coordinate system to apply the corresponding sensing feedback force to the doctor's operation sensing. Specifically,

[0159] To achieve the purpose of the master hand sensing feedback when the instrument end contacts the human tissue, in combination with the master-slave operation consistency, the base coordinate system is taken as the reference coordinate system, and the force direction (i.e. contact force direction) vector of the n-degree-of-freedom slave end mechanical arm instrument end contacting the human tissue relative to the base coordinate system is consistent with the sensing feedback direction (i.e. feedback force direction) of the master hand relative to the base coordinate system. According to the above model result, in combination with the master-slave operation consistency, the feedback force direction of the sensing feedback force provided by the master hand is The feedback force size of the sensing feedback force is self-adaptive to the contact force size.

[0160] According to the relationship model of joint torque and generalized spatial force, it can be known that:

[0161] τ = J T F;

[0162] Wherein, J T is the Jacobian matrix of the master hand; τ is the joint sensing torque, which is represented by the following formula in the example, τ 位1-1 —τ 位1-n corresponds to the n position joints in the master hand respectively, τ 姿1-1 —τ 姿1-n corresponds to the n attitude joints in the master hand respectively:

[0163]

[0164] F is the sensing feedback force applied to the master hand, in the example, according to the force direction and size of the slave end mechanical arm relative to the human tissue, the sensing feedback force relative to the base coordinate system is applied to the master hand, which has the same direction and self-adaptive size, wherein K coe is the self-adaptive sensing coefficient:

[0165]

[0166] The joint torque τ 位1-1 , τ 位1-2 , …, τ 位1-n , τ 姿1-1 , …, τ 姿1-n under the model can be obtained.

[0167] Further, according to the impedance torque M of each slave arm obtained above, the impedance torque is applied to each posture joint, the operation intention of the doctor is prompted as a dangerous behavior, and the doctor is guided to perform the master-slave operation in a safe direction.

[0168] Considering the movement direction of the motor shaft of the instrument relative to the base coordinate system (i.e., the impedance torque direction), the slave arm impedance torque model relative to the base coordinate system can be obtained as follows:

[0169]

[0170] According to the consistency of the master-slave operation, the slave arm impedance torque model generates and applies the adaptive master hand impedance torque to the master hand end relative to the base coordinate system, so that the doctor can perceive the operation.

[0171] According to the relationship model between the joint torque and the generalized spatial force, it can be known that:

[0172] τ = J T F;

[0173] wherein J T is the Jacobian matrix of the master operating hand; τ is the joint impedance torque, which is represented by the following formula in the example, τ 位2-1 - τ 位2-n corresponds to the n position joints in the master operating hand, and τ 姿2-1 - τ 姿2-n corresponds to the n posture joints in the master operating hand:

[0174]

[0175] F is the master hand impedance torque, which is represented by the following formula:

[0176]

[0177] The joint impedance torque τ 姿2-1 , …, τ 姿2-n under the slave arm impedance torque model can be obtained.

[0178] Then, the joint torques are added to obtain the master-slave safe operation torque:

[0179]

[0180] The above example can effectively guide the doctor to operate safely, and greatly avoid damage to human tissues.

[0181] The above specific embodiments do not constitute a limitation on the protection scope of the present application. 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 modification, equivalent replacement, and improvement made within the spirit and principles of the present application shall be included in the protection scope of the present application.

Claims

1. A robot master manipulator control system, characterized in that, The application relates to a robot system, comprising: a master controller and a slave controller integrated on a robot, the master controller corresponding to a master operating hand in the robot, and the slave controller corresponding to a slave end mechanical arm in the robot, and a target instrument being installed on the slave end mechanical arm; wherein the slave controller is used for determining slave arm impedance torque generated by an instrument joint of an instrument end of the target instrument in the case that an applied contact force on the target object by the instrument end is greater than a safe contact force that the target object can bear, and sending the slave arm impedance torque to the master controller; the master controller is used for determining joint impedance torque applied on a master hand joint of the master operating hand according to the slave arm impedance torque, and controlling the master operating hand to move according to the joint impedance torque.

2. The system of claim 1, wherein, The slave controller is further used for: determining an actual output current value of an instrument motor of the instrument joint in a current period, a previous output current value of the instrument motor in a previous period of the current period, and a theoretical output current value of the instrument motor in an idle state; determining a change of the contact force according to an absolute value of a first difference between the actual output current value and the theoretical output current value, and a second difference between the actual output current value and the previous output current value; The slave controller is specifically used for: determining the slave arm impedance torque generated by the instrument joint of the instrument end according to the first difference and the second difference in the case that the change represents that the applied contact force is increased, and taking zero as the slave arm impedance torque in the case that the change represents that the applied contact force is decreased; sending the slave arm impedance torque to the master controller.

3. The system of claim 2, wherein, The slave controller is further used for: determining an impedance torque direction of the slave arm impedance torque according to forward kinematics of the mechanical arm for the slave arm impedance torque to be determined; The slave controller is specifically used for: determining an impedance torque size of the slave arm impedance torque according to the first difference and the second difference in the case that the change represents that the applied contact force is increased; determining the slave arm impedance torque according to the impedance torque direction and the impedance torque size.

4. The system of claim 2, wherein, The slave controller is specifically used for: obtaining an angle state of the instrument joint according to data recorded in a motor encoder of the instrument motor in the case that the instrument motor is in the idle state; obtaining the theoretical output current value of the instrument motor according to the angle state.

5. The system of claim 1, wherein, The slave controller is further used for: determining an absolute value of a first difference between an actual output current value of the instrument motor and a theoretical output current value of the instrument motor in an idle state; obtaining a contact threshold value preset for the safe contact force, and determining that the applied contact force is greater than the safe contact force in the case that the absolute value is greater than the contact threshold value.

6. The system of claim 1, wherein, The slave controller is further used for: determining the applied contact force, and sending the applied contact force to the master controller; the master controller is further configured to: determine a perceived feedback force applied on the master manipulator according to the applied contact force, wherein a contact force direction of the applied contact force is the same as a feedback force direction of the perceived feedback force; determine a joint perceived moment applied on the master joint according to the perceived feedback force; the master controller is specifically configured to: control the master manipulator motion according to the joint perceived moment and the joint impedance moment.

7. The system of claim 6, wherein, the slave controller is specifically configured to: determine the contact force magnitude and the contact force direction of the applied contact force for the applied contact force to be determined, and determine the applied contact force according to the contact force magnitude and the contact force direction; send the applied contact force to the master controller.

8. The system of claim 7, wherein, the slave controller is specifically configured to: determine an absolute value of a first difference between an actual output current value of the instrument motor and a theoretical output current value of the instrument motor in an idle state for the instrument motor of the instrument joint; obtain a preset motor current-moment coefficient for the instrument joint, and determine the contact force magnitude of the applied contact force according to the motor current-moment coefficient and the absolute value.

9. The system of claim 7, wherein, the slave controller is specifically configured to: determine an actual position of the instrument tip, and calculate a theoretical position of the instrument tip according to the master manipulator motion forward solution and a master-slave operation ratio; determine the contact force direction of the applied contact force according to a position difference between the actual position and the theoretical position.

10. The system of claim 6, wherein, the master controller is specifically configured to: control the master manipulator motion according to a position component and a perceived attitude component in the joint perceived moment, and an impedance attitude component in the joint impedance moment.

11. The system of any one of claims 1-10, wherein, the robot comprises a surgical robot, and the target instrument is a surgical instrument; and / or a joint type of the instrument joint comprises at least one of a swing joint, a pitch joint and a yaw joint, and a joint type of the master joint corresponds to the joint type of the instrument joint.

12. A robot, characterized in that comprise: a master manipulator, a slave end manipulator and the robot master manipulator control system in any one of claims 1-11.

Images