Robot control method and device, robot system and storage medium

Abstract

The invention relates to a robot control method and device, a robot system and a storage medium, and the method comprises the steps: according to the joint torque of a robot in a current control period, robot motion information and a predetermined acceleration constraint condition of the robot in a next control period, determining the joint torque of the robot in the next control period; the joint acceleration of the robot in the next control period is determined; the acceleration constraint condition is determined according to an operation space constraint of the robot and a joint space constraint of the robot, and the operation space constraint and the joint space constraint are determined according to a constraint space of the robot; according to the joint acceleration of the robot in the next control period, the joint torque of the robot in the next control period is determined; and controlling the robot according to the joint torque of the robot in the next control period. The technical problem that the movement accuracy of the surgical instrument at the tail end of the robot in the limited space cannot be guaranteed is solved.

Description

Technical Field

[0001] The present application relates to the field of robot control, and in particular to a robot control method, device, robot system and storage medium. Background Art

[0002] During the working process of medical robots, in order to improve safety and intelligent interactivity, the application of virtual fixture technology is becoming more and more extensive, that is, the working space of the robot's end surgical instruments is limited as needed, so that the robot can move in a confined space and complete the operation safely.

[0003] Existing robot control methods do not take into account the limitations of both the operating space constraints and the joint space constraints, and thus cannot guarantee the accuracy of the robot's end surgical instrument's movement in a confined space. Summary of the invention

[0004] The present application provides a robot control method, device, robot system and storage medium to solve the problem that the existing robot control method does not take into account the limitations of operating space constraints and joint space constraints at the same time, and thus cannot ensure the accuracy of the movement of the robot's end surgical instrument in a confined space.

[0005] In a first aspect, a robot control method is provided in the present application, the method comprising:

[0006] Determine the joint acceleration of the robot in the next control cycle according to the joint torque of the robot in the current control cycle, the robot motion information and the predetermined acceleration constraint of the robot in the next control cycle; the acceleration constraint is determined according to the operation space constraint of the robot and the joint space constraint of the robot, and the operation space constraint and the joint space constraint are determined according to the constraint space of the robot;

[0007] Determining the joint torque of the robot in the next control cycle according to the joint acceleration of the robot in the next control cycle;

[0008] The robot is controlled according to the joint torque of the robot in the next control cycle.

[0009] In some embodiments, the method further comprises:

[0010] The acceleration constraint condition of the robot in the next control cycle is determined according to the operation space constraint of the robot, the joint space constraint of the robot, and the relationship between the robot motion information of the current control cycle and the robot motion information of the next control cycle.

[0011] In some of the embodiments, the robot motion information includes joint positions and joint velocities;

[0012] Determining the joint acceleration of the robot in the next control cycle according to the joint torque of the robot in the current control cycle, the robot motion information and the predetermined acceleration constraint condition of the robot in the next control cycle includes:

[0013] Determine the joint acceleration and terminal acceleration of the robot in the current control cycle according to the joint torque, joint position and joint velocity of the robot in the current control cycle;

[0014] The joint acceleration of the robot in the next control cycle is determined according to the joint acceleration of the robot in the current control cycle, the terminal acceleration and the predetermined acceleration constraint condition of the robot in the next control cycle.

[0015] In some embodiments, determining the joint acceleration and terminal acceleration of the robot in the current control cycle according to the joint torque, joint position and joint velocity of the robot in the current control cycle includes:

[0016] Calculating the joint acceleration of the robot in the current control cycle using the dynamics model of the robot according to the joint torque, joint external force, joint position and joint velocity of the robot in the current control cycle;

[0017] Obtain the mapping relationship between the robot's joint acceleration and terminal acceleration;

[0018] The terminal acceleration of the robot in the current control cycle is calculated according to the joint acceleration of the robot in the current control cycle and the mapping relationship.

[0019] In some embodiments, determining the joint acceleration of the robot in the next control cycle according to the joint acceleration, the terminal acceleration of the robot in the current control cycle and the predetermined acceleration constraint of the robot in the next control cycle includes:

[0020] Determining the terminal acceleration of the robot in the next control cycle according to the terminal acceleration of the robot in the current control cycle and the predetermined acceleration constraint condition of the robot in the next control cycle;

[0021] The joint acceleration of the robot in the next control cycle is determined according to the terminal acceleration of the robot in the next control cycle.

[0022] In some embodiments, determining the joint torque of the robot in the next control cycle according to the joint acceleration of the robot in the next control cycle includes:

[0023] The joint torque of the robot in the next control cycle is determined according to the joint position and joint velocity of the robot in the current control cycle and the joint acceleration of the robot in the next control cycle.

[0024] In some embodiments, determining the joint acceleration of the robot in the next control cycle according to the joint acceleration, the terminal acceleration of the robot in the current control cycle and the predetermined acceleration constraint of the robot in the next control cycle includes:

[0025] Determining the terminal acceleration of the robot in the next control cycle according to the terminal acceleration of the robot in the current control cycle and the predetermined acceleration constraint condition of the robot in the next control cycle;

[0026] According to the terminal acceleration of the robot in the next control cycle, using the dynamic model of the robot, determining the initial joint acceleration of the robot in the next control cycle;

[0027] The joint acceleration of the robot in the next control cycle is determined according to the initial joint acceleration of the robot in the next control cycle and the predetermined acceleration constraint condition of the robot in the next control cycle.

[0028] In some of the embodiments, the geometry of the constraint space of the robot is determined according to a surgical area and a target tissue corresponding to the surgical area.

[0029] In a second aspect, a robot control device is provided in the present application, the device comprising:

[0030] A first determination module is used to determine the joint acceleration of the robot in the next control cycle according to the joint torque of the robot in the current control cycle, the robot motion information and the predetermined acceleration constraint of the robot in the next control cycle; the acceleration constraint is determined according to the operation space constraint of the robot and the joint space constraint of the robot, and the operation space constraint and the joint space constraint are determined according to the constraint space of the robot;

[0031] A second determination module, used to determine the joint torque of the robot in the next control cycle according to the joint acceleration of the robot in the next control cycle;

[0032] An execution module is used to control the robot according to the joint torque of the robot in the next control cycle.

[0033] According to a third aspect, a robot system is provided in the present application, the system comprising a robot, the robot comprising a robotic arm, the robot being used to execute the robot control method according to the first aspect, and to perform motion control on the robotic arm of the robot via the method.

[0034] In a fourth aspect, the present application provides a computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the steps of the robot control method described in the first aspect.

[0035] Compared with the prior art, a robot control method, device, robot system and storage medium provided in the present application determine the acceleration constraint conditions of the next control cycle according to the robot's operating space constraints and the robot's joint space constraints, determine the joint acceleration of the robot in the next control cycle according to the joint torque of the robot in the current control cycle, the robot motion information and the predetermined acceleration constraint conditions of the robot in the next control cycle, determine the joint torque of the robot in the next control cycle according to the joint acceleration of the robot in the next control cycle, control the robot according to the joint torque of the robot in the next control cycle, and control the robot while considering the limitations of the operating space constraints and the joint space constraints, thereby solving the technical problem that the existing robot control method does not consider the limitations of the operating space constraints and the joint space constraints at the same time, and cannot ensure the accuracy of the movement of the robot's end surgical instrument in a confined space.

[0036] Details of one or more embodiments of the present application are set forth in the following drawings and description to make other features, objects, and advantages of the present application more readily apparent. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] The drawings described herein are used to provide a further understanding of the present application and constitute a part of the present application. The illustrative embodiments of the present application and their descriptions are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings:

[0038] Figure 1 is a hardware structure block diagram of a terminal for executing a robot control method according to an embodiment of the present application;

[0039] Figure 2 is a flow chart of a robot control method according to an embodiment of the present application;

[0040] Figure 3 is a schematic diagram of a robot system based on torque control according to this specific embodiment;

[0041] Figure 4 is a flow chart of a robot control method of this specific embodiment;

[0042] Figure 5 is a schematic diagram of the constraint space of this specific embodiment;

[0043] Figure 6 It is a structural block diagram of a robot control device according to an embodiment of the present application. DETAILED DESCRIPTION

[0044] In order to more clearly understand the purpose, technical solutions and advantages of the present application, the present application is described and illustrated below in conjunction with the accompanying drawings and embodiments.

[0045] Unless otherwise defined, the technical terms or scientific terms involved in this application shall have the general meaning understood by people with ordinary skills in the technical field to which this application belongs. The words "one", "a", "a", "the", "these" and the like in this application do not represent quantitative restrictions, and they can be singular or plural. The terms "include", "comprise", "have" and any variants thereof involved in this application are intended to cover non-exclusive inclusions; for example, a process, method and system, product or device comprising a series of steps or modules (units) is not limited to the listed steps or modules (units), but may include unlisted steps or modules (units), or may include other steps or modules (units) inherent to these processes, methods, products or devices. The words "connect", "connected", "coupled" and the like involved in this application are not limited to physical or mechanical connections, but may include electrical connections, whether directly or indirectly. The "multiple" involved in this application refers to two or more. "And / or" describes the association relationship of associated objects, indicating that there can be three relationships. For example, "A and / or B" can mean: A exists alone, A and B exist at the same time, and B exists alone. Usually, the character " / " indicates that the objects associated with each other are in an "or" relationship. The terms "first", "second", "third", etc. involved in this application are only used to distinguish similar objects and do not represent a specific ordering of the objects.

[0046] The method embodiments provided in this application can be executed in a terminal, a computer or a similar computing device. For example, running on a terminal, Figure 1 1 is a hardware structure block diagram of a terminal for executing a robot control method according to an embodiment of the present application. Figure 1 As shown, the terminal may include one or more ( Figure 1Only one is shown in the figure) processor 102 and memory 104 for storing data, wherein processor 102 may include but is not limited to a processing device such as a microprocessor MCU or a programmable logic device FPGA. The above terminal may also include a transmission device 106 and an input and output device 108 for communication functions. It can be understood by those skilled in the art that Figure 1 The structure shown is only for illustration and does not limit the structure of the above terminal. Figure 1 More or fewer components as shown, or with Figure 1 Different configurations shown.

[0047] The memory 104 can be used to store computer programs, for example, software programs and modules of application software, such as a computer program corresponding to a robot control method in this embodiment. The processor 102 executes various functional applications and data processing by running the computer program stored in the memory 104, that is, to implement the above method. The memory 104 may include a high-speed random access memory, and may also include a non-volatile memory, such as one or more magnetic storage devices, flash memory, or other non-volatile solid-state memory. In some examples, the memory 104 may further include a memory remotely arranged relative to the processor 102, and these remote memories may be connected to the terminal via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.

[0048] The transmission device 106 is used to receive or send data via a network. The above network includes a wireless network provided by the communication provider of the terminal. In one example, the transmission device 106 includes a network adapter (Network Interface Controller, referred to as NIC), which can be connected to other network devices through a base station so as to communicate with the Internet. In one example, the transmission device 106 can be a radio frequency (Radio Frequency, referred to as RF) module, which is used to communicate with the Internet wirelessly.

[0049] In this application, a robot control method is provided. Figure 2 is a flow chart of a robot control method according to an embodiment of the present application, such as Figure 2 As shown, the process includes the following steps:

[0050] Step S210, determines the joint acceleration of the robot in the next control cycle according to the joint torque of the robot in the current control cycle, the robot motion information and the predetermined acceleration constraint conditions of the robot in the next control cycle; the acceleration constraint conditions are determined according to the robot's operating space constraints and the robot's joint space constraints, and the operating space constraints and the joint space constraints are determined according to the robot's constraint space.

[0051] Specifically, according to the robot's operating space constraint, the robot's joint space constraint, and the relationship between the robot motion information of the current control cycle and the robot motion information of the next control cycle, the robot's acceleration constraint condition in the next control cycle is determined, and the operating space constraint and the joint space constraint are determined according to the robot's constraint space, i.e., the operating restricted space. Further specifically, the robot motion information of the next control cycle is estimated according to the robot motion information of the current control cycle, and the estimated robot motion information of the next control cycle is substituted into the robot's operating space constraint and the robot's joint space constraint to determine the robot's acceleration constraint condition in the next control cycle, wherein the acceleration constraint condition includes the robot's terminal acceleration constraint condition and the robot's joint acceleration constraint condition, the operating space constraint is a Cartesian space constraint, i.e., the terminal constraint, the joint space constraint is the robot's own joint constraint, the operating space constraint includes a Cartesian position constraint, a Cartesian velocity constraint, and a Cartesian acceleration constraint, and the joint space constraint includes a joint position constraint, a joint velocity constraint, and a joint acceleration constraint.

[0052] It should be noted that the robot motion information here includes the robot's end position, the robot's end position maximum value, the robot's end position minimum value, the robot's end velocity, the robot's end velocity maximum value, the robot's end velocity minimum value, the robot's end acceleration, the robot's end acceleration maximum value, the robot's end acceleration minimum value, the robot's joint position, the robot's joint position maximum value, the robot's joint position minimum value, the robot's joint velocity, the robot's joint velocity maximum value, the robot's joint velocity minimum value, the robot's joint acceleration, the robot's joint acceleration maximum value, the robot's joint acceleration minimum value and other motion information.

[0053] Step S220, determining the joint torque of the robot in the next control cycle according to the joint acceleration of the robot in the next control cycle.

[0054] Specifically, the joint torque of the robot in the next control cycle is determined according to the joint position of the robot in the current control cycle, the joint velocity of the current control cycle and the joint acceleration of the robot in the next control cycle determined in step S210.

[0055] Step S230, controlling the robot according to the joint torque of the robot in the next control cycle.

[0056] Specifically, the joint torque of the robot can determine the joint acceleration of the robot through forward dynamics, and the robot is controlled according to the determined joint acceleration, so that the corresponding robot operation space constraints and robot joint space constraints are satisfied in each control cycle.

[0057] In this embodiment, the acceleration constraint condition of the next control cycle is determined according to the operation space constraint of the robot and the joint space constraint of the robot, the joint acceleration of the robot in the next control cycle is determined according to the joint torque of the robot in the current control cycle, the robot motion information and the predetermined acceleration constraint condition of the robot in the next control cycle, the joint torque of the robot in the next control cycle is determined according to the joint acceleration of the robot in the next control cycle, the robot is controlled according to the joint torque of the robot in the next control cycle, and the robot is controlled by considering the limitations of the operation space constraint and the joint space constraint, thereby solving the technical problem that the existing robot control method does not consider the limitations of the operation space constraint and the joint space constraint at the same time and cannot ensure the accuracy of the movement of the robot's end surgical instrument in a confined space.

[0058] In some of the embodiments, the joint acceleration of the robot in the next control cycle is determined based on the joint torque of the robot in the current control cycle, the robot motion information and the predetermined acceleration constraint conditions of the robot in the next control cycle, including: determining the joint acceleration and terminal acceleration of the robot in the current control cycle based on the joint torque, joint position and joint velocity of the robot in the current control cycle; determining the joint acceleration of the robot in the next control cycle based on the joint acceleration, terminal acceleration of the robot in the current control cycle and the predetermined acceleration constraint conditions of the robot in the next control cycle.

[0059] In some of the embodiments, the joint acceleration and terminal acceleration of the robot in the current control cycle are determined according to the joint torque, joint position and joint velocity of the robot in the current control cycle, including: calculating the joint acceleration of the robot in the current control cycle using the robot's dynamic model according to the joint torque, joint external force, joint position and joint velocity of the robot in the current control cycle; obtaining the mapping relationship between the joint acceleration and the terminal acceleration of the robot; calculating the terminal acceleration of the robot in the current control cycle according to the joint acceleration of the robot in the current control cycle and the obtained mapping relationship between the joint acceleration and the terminal acceleration of the robot.

[0060] In some embodiments, determining the joint acceleration of the robot in the next control cycle according to the joint acceleration of the robot in the current control cycle, the terminal acceleration and the predetermined acceleration constraint of the robot in the next control cycle includes:

[0061] Determine the terminal acceleration of the robot in the next control cycle according to the terminal acceleration of the robot in the current control cycle and the predetermined acceleration constraint condition of the robot in the next control cycle;

[0062] The joint acceleration of the robot in the next control cycle is determined according to the terminal acceleration of the robot in the next control cycle.

[0063] In some of the embodiments, the joint acceleration of the robot in the next control cycle is determined based on the joint acceleration, terminal acceleration and predetermined acceleration constraints of the robot in the next control cycle in the current control cycle, including: determining the terminal acceleration of the robot in the next control cycle based on the terminal acceleration of the robot in the current control cycle and predetermined acceleration constraints of the robot in the next control cycle; determining the initial joint acceleration of the robot in the next control cycle using the dynamic model of the robot based on the terminal acceleration of the robot in the next control cycle; determining the joint acceleration of the robot in the next control cycle based on the initial joint acceleration of the robot in the next control cycle and predetermined acceleration constraints of the robot in the next control cycle.

[0064] In some of the embodiments, the geometry of the robot's constraint space is determined according to the surgical area and the target tissue corresponding to the surgical area.

[0065] Specifically, the robot's constraint space is a safe operating space determined according to the surgical area and the target tissue corresponding to the surgical area, and the geometric shape of the safe operating space is determined according to the geometric shape of the surgical area and the target tissue corresponding to the surgical area. The target tissue here can be tissues such as the knee joint and the pelvis. Depending on the surgical area and the target tissue corresponding to the surgical area, the geometric shape of the robot's constraint space is different. For example, for the pelvis, the robot's constraint space can be a conical constraint space, for the knee joint, the robot's constraint space can be a plane constraint space, and for other tissues, the robot's constraint space can also be a rectangular constraint space.

[0066] The present application provides a robot, which is used to execute the robot control method described in the aforementioned embodiment.

[0067] Specifically, the robot includes a controller and a terminal end of the robot, the controller is connected to the terminal end of the robot, and the controller executes the robot control method described in the embodiment to control the terminal end of the robot to accurately move in a constrained space.

[0068] In this embodiment, the robot determines the acceleration constraint conditions of the next control cycle according to the operating space constraints of the robot and the joint space constraints of the robot, determines the joint acceleration of the robot in the next control cycle according to the joint torque of the robot in the current control cycle, the robot motion information and the predetermined acceleration constraint conditions of the robot in the next control cycle, determines the joint torque of the robot in the next control cycle according to the joint acceleration of the robot in the next control cycle, controls the robot according to the joint torque of the robot in the next control cycle, and controls the robot by considering the limitations of the operating space constraints and the joint space constraints at the same time, thereby solving the technical problem that the existing robots do not consider the limitations of the operating space constraints and the joint space constraints at the same time, and cannot ensure the accuracy of the movement of the robot's end surgical instruments in a confined space.

[0069] A robot system is provided in the present application. The robot system includes a robot. The robot includes a robotic arm. The robot is used to execute the robot control method described in the aforementioned embodiment, and motion control of the robotic arm of the robot is performed via the method.

[0070] In this embodiment, the robot system determines the acceleration constraint conditions of the next control cycle based on the robot's operating space constraints and the robot's joint space constraints, determines the joint acceleration of the robot in the next control cycle based on the joint torque of the robot in the current control cycle, the robot motion information and the predetermined acceleration constraint conditions of the robot in the next control cycle, determines the joint torque of the robot in the next control cycle based on the joint acceleration of the robot in the next control cycle, controls the robot based on the joint torque of the robot in the next control cycle, and controls the robot while considering the limitations of the operating space constraints and the joint space constraints, thereby solving the technical problem that the existing robot system does not consider the limitations of the operating space constraints and the joint space constraints at the same time and cannot ensure the accuracy of the movement of the robot's end surgical instruments in a confined space.

[0071] The embodiments of the present application are described and illustrated below through specific examples.

[0072] Taking the application of total hip replacement surgery as an example, the purpose of this surgery is to replace the diseased hip joint with a joint prosthesis. During the surgery, the robot system needs to ensure the safety and accuracy of the operation. Specifically, the acetabular grinding file accurately grinds out the planned hemispherical volume without damaging the target tissue. In order to achieve the safety and accuracy requirements during the operation, this specific embodiment provides a robot system based on torque control. The robot system is used to assist doctors in operating, such as Figure 3 As shown, the robot system includes:

[0073] Constraint space 1, a safe operation space determined according to the surgical area and target tissue, can be a conical constraint area. The constraint space 1 is a virtual geometric area in which position / velocity / acceleration constraints can be defined; when the doctor drags the tool to different positions, the robot arm 2 will apply corresponding feedback force according to the constraint conditions to prompt and correct the doctor's operation error.

[0074] The robot arm 2 is used to execute the robot control method described in the above embodiment to control the posture of the tool 3, constrain the state of the tool 3, and provide force feedback to achieve precise operation. The tool 3 here is the end of the robot described in the above embodiment, and the robot arm 2 here is the robot described in the above embodiment.

[0075] Tool 3 is used to rasp the volume of the acetabular hemisphere.

[0076] The cart array 4 is mounted on a cart 7 fixedly connected to the robotic arm 2 and is used to determine the position of the robotic arm base in space in conjunction with the navigation device 6.

[0077] The pelvic array 5 is mounted on the patient's pelvis and is used to determine the position of the pelvis in space in conjunction with the navigation device 6 .

[0078] The navigation device 6 is used to locate the position of the robot arm base and the patient's pelvis during surgery.

[0079] In this embodiment, the robot system determines the acceleration constraint conditions of the next control cycle based on the robot's operating space constraints and the robot's joint space constraints, determines the joint acceleration of the robot in the next control cycle based on the joint torque of the robot in the current control cycle, the robot motion information and the predetermined acceleration constraint conditions of the robot in the next control cycle, determines the joint torque of the robot in the next control cycle based on the joint acceleration of the robot in the next control cycle, controls the robot based on the joint torque of the robot in the next control cycle, and controls the robot while considering the limitations of the operating space constraints and the joint space constraints, thereby solving the technical problem that the existing robot system does not consider the limitations of the operating space constraints and the joint space constraints at the same time and cannot ensure the accuracy of the movement of the robot's end surgical instruments in a confined space.

[0080] Figure 4 FIG. 1 is a flow chart of a robot control method of this specific embodiment. Figure 4 As shown, the process includes the following steps:

[0081] Step S410, determining constraint conditions and converting them into acceleration constraint conditions.

[0082] Specifically, the acceleration constraints include the end acceleration constraints and the joint acceleration constraints. The position, velocity and acceleration can be linked and the constraints can be converted through the forward and backward control cycles. The joint torque of the manipulator can determine the joint acceleration through forward dynamics. Similarly, the required joint torque can be determined through inverse dynamics according to the state of the joint of the manipulator (position, velocity). The acceleration layer constraints are more intuitive and effective in feeding back to the control instructions, and compared with controlling the joint position / velocity, the dragging of the joint torque is smoother.

[0083] More specifically, the constraint space includes Cartesian space constraints (task constraints, end constraints) and joint space constraints (bottom-level implementation, robot joint constraints). The general constraint relationship is expressed as shown in formula (1):

[0084]

[0085] Among them, x, are the Cartesian position, Cartesian velocity and Cartesian acceleration, x, It can also be called the end position, end velocity and end acceleration, that is, the Cartesian space constraint includes the Cartesian position constraint, the Cartesian velocity constraint and the Cartesian acceleration constraint, q, The joint position, joint velocity and joint acceleration are respectively, that is, the joint space constraints include joint position constraints, joint velocity constraints and joint acceleration constraints, x min 、x max are the minimum and maximum Cartesian positions, respectively, are the minimum and maximum values ​​of Cartesian velocity, are the minimum and maximum values ​​of Cartesian acceleration, q min ,q max are the minimum and maximum values ​​of the joint positions, respectively. are the minimum and maximum joint velocities, respectively. are the minimum and maximum values ​​of joint acceleration respectively. Formula (1) is a general constraint relationship, which can also be called the initial constraint relationship.

[0086] In order to constrain the tool to stop at the boundary, it is also necessary to satisfy that the velocity and acceleration are 0 when the position is at the boundary. Based on this, constraints can be added to ensure stability at the boundary.

[0087] Estimate the speed and position at the next moment (next control cycle) through the current moment (current control cycle):

[0088]

[0089]

[0090]

[0091]

[0092] Where T is the control period, x k , q k , They are the end position, end velocity, joint position, and joint velocity at the current moment, respectively. k+1 , q k+1 , They are the estimated states at the next moment, namely the end position, end velocity, joint position, and joint velocity at the next moment. An optional method is to use limited acceleration. Limiting the acceleration can better ensure the smoothness of the movement.

[0093] Substituting equations (2) to (5) into equation (1), we can obtain the acceleration constraints of the Cartesian space and the acceleration constraints of the joint space, that is, the acceleration constraints of the end and the acceleration constraints of the joint, as shown in equation (6):

[0094]

[0095] Wherein, min is the minimum value, max is the maximum value, formula (1) is the basic constraint determined according to the constraint space, and formula (6) is the acceleration constraint condition obtained by converting the position, velocity and acceleration in the previous and next control cycles according to formula (1). The acceleration constraint condition of the end and the acceleration constraint condition of the joint determined by formula (6) are the acceleration constraints of the robot in the next control cycle as predetermined in the above embodiment. The acceleration constraint condition of the end and the acceleration constraint condition of the joint are: the minimum value of the acceleration of the end is The maximum value of the three, the maximum value of the terminal acceleration is The minimum value of the three; the minimum value of joint acceleration is is the maximum value of the three, and the maximum value of joint acceleration is is the minimum value among the three.

[0096] Step S420, obtaining the current state of the robot arm and calculating the acceleration according to the torque.

[0097] Specifically, the current state of the manipulator (position and velocity of the end and joint) is obtained, which includes the end position, end velocity, joint position and joint velocity, and the end acceleration and joint acceleration are calculated according to the joint command of the previous moment. The joint position can be obtained through the manipulator joint encoder, and the joint velocity is obtained by difference. The end position is obtained by forward kinematics, and the end velocity is obtained by multiplying the Jacobian matrix with the joint velocity. Since there is a lot of noise when the acceleration is directly calculated by position difference, the joint acceleration and the end acceleration are calculated by forward dynamics here. Of course, an optional method is to use Kalman filtering to process the joint angle to obtain the joint acceleration.

[0098] Specifically, the controller enters the initial static state, corresponding to The state in each control cycle satisfies the condition by iterative constraints. Here, the iterative constraints refer to the looping of steps S410 to S440, and each cycle corresponds to one loop. The controller only needs to compensate for gravity and Coriolis force (or only gravity) in free space, corresponding to τ=g+c, where τ, c, and g are the joint output torque, Coriolis force, and gravity, respectively.

[0099] According to the current joint position, joint velocity, joint external force (environmental external force) and joint torque command of the previous cycle (an optional method is to directly use the actual force detected by the sensor), the joint acceleration and terminal acceleration of the current control cycle are calculated through the dynamic model. According to the joint torque, joint external force, joint position and joint velocity of the robot in the current control cycle, the joint acceleration of the robot in the current control cycle is calculated using the dynamic model of the robot. According to the joint acceleration of the robot in the current control cycle and the mapping relationship between the joint acceleration and the terminal acceleration, the terminal acceleration of the robot in the current control cycle is calculated. Among them, the mapping relationship between joint acceleration and terminal acceleration can be derived from the mapping relationship between joint velocity and terminal velocity. The mapping relationship between joint velocity and terminal velocity is: The mapping relationship between joint acceleration and terminal acceleration can be Where J is the Jacobian matrix.

[0100] Step S430, limiting the end acceleration and the joint acceleration to determine the joint acceleration of the robot arm in the next control cycle.

[0101] Specifically, the terminal acceleration and joint acceleration are limited, and the boundary value is used for those exceeding the limit; due to the limited output power and torque of the robot arm joint, the calculated expected joint acceleration or terminal acceleration may exceed the motor output capacity. Here, the expected acceleration is limited to the motor output capacity to ensure effective output. An optional method is to determine the range of robot arm joint acceleration and terminal acceleration based on common configurations and motor output capacity, and take the minimum value as the limiting constraint. Another optional method is to collect the actual joint speed and terminal speed limits in multiple processes and take the narrowest range based on this range or select it according to a certain ratio. Another optional method is to calculate the shutdown acceleration and terminal acceleration values ​​as thresholds based on the output capacity and current state of the motor, or select it according to a certain ratio and make changes in each cycle to maximize the use of the motor's output capacity.

[0102] Specifically, the terminal acceleration and joint acceleration are limited, the input terminal acceleration is constrained during iteration, and the expected terminal acceleration value is calculated, as shown in formula (7):

[0103]

[0104] Constrain the input terminal acceleration to obtain the expected terminal acceleration value, which is The maximum value among the four, is the input terminal acceleration.

[0105] The corresponding initial expected joint acceleration is solved according to the expected terminal acceleration and the dynamic model, and the solved initial expected joint acceleration is constrained to obtain the constrained expected joint acceleration value, as shown in formula (8):

[0106]

[0107] The initial desired joint acceleration is constrained to obtain the desired joint acceleration value after constraint. The desired joint acceleration value after constraint is is the maximum value among the four, among which, is the initial desired joint acceleration to be solved.

[0108] The expected terminal acceleration here is the terminal acceleration of the robot described in the aforementioned embodiment in the next control cycle, the initial expected joint acceleration here is the initial joint acceleration of the robot described in the aforementioned embodiment in the next control cycle, and the constrained expected joint acceleration value here is the joint acceleration of the robot described in the aforementioned embodiment in the next control cycle.

[0109] According to the terminal acceleration of the manipulator in the current control cycle and the acceleration constraint of the terminal in formula (6), the terminal acceleration of the manipulator in the next control cycle is determined; according to the terminal acceleration of the manipulator in the next control cycle, the initial joint acceleration of the manipulator in the next control cycle is determined by using the dynamic model of the manipulator; according to the initial joint acceleration of the manipulator in the next control cycle and the acceleration constraint of the joint in formula (6), the joint acceleration of the manipulator in the next control cycle is determined.

[0110] Step S440, solving the expected joint torque according to the current position, speed and the expected joint acceleration after constraint and sending it to control the robotic arm.

[0111] Specifically, the expected joint torque is solved according to the current position, speed and expected joint acceleration after constraints and sent to control the robot arm. Iterative control is performed from the initial state to ensure that the constraints are met during the operation of the controller. The control frequency is 1000Hz, and it will automatically slow down according to the constraints near the boundary to ensure the constraints. More specifically, the corresponding expected joint torque is calculated by inverse dynamics according to the current joint position, joint speed and expected joint acceleration after constraints, and the calculated expected joint torque is sent to control the robot arm. The calculated joint torque is used as the initial value of the new round of iterative joint torque to ensure that the corresponding Cartesian space and joint space constraints are met in each control cycle. The joint torque of each control cycle is calculated and sent to constrain the tool or end within a safe range to ensure the smoothness and safety of the doctor's operation.

[0112] The above-mentioned embodiment performs restrictions at the acceleration layer of the robot arm, and corresponding restrictions may also be performed at the acceleration layer or the speed layer, and finally converted into the joint torque of the robot arm and sent down.

[0113] In this embodiment, the acceleration constraint condition of the next control cycle is determined according to the operating space constraint of the robot arm and the joint space constraint of the robot arm, the joint acceleration of the robot arm in the next control cycle is determined according to the joint torque of the robot arm in the current control cycle, the robot arm motion information and the predetermined acceleration constraint condition of the robot arm in the next control cycle, the joint torque of the robot arm in the next control cycle is determined according to the joint acceleration of the robot arm in the next control cycle, the robot arm is controlled according to the joint torque of the robot arm in the next control cycle, and the robot arm is controlled by considering the limitations of the operating space constraint and the joint space constraint at the same time, thereby solving the technical problem that the existing robot arm control method does not consider the limitations of the operating space constraint and the joint space constraint at the same time, and cannot ensure the accuracy of the movement of the end surgical instrument of the robot arm in a confined space.

[0114] In some of the embodiments, the geometric shape of the constraint space is determined according to the surgical area and the target tissue, and can be a funnel-shaped constraint space, a cone-shaped constraint space, a plane constraint space, a rectangular constraint space, etc. Figure 5 The figure shows three types of constraint spaces: cone constraint space, plane constraint space and rectangular constraint space. More complex constraints can be expanded through basic constraints.

[0115] The constraints of the conical constraint space only need to refine the general constraints. Figure 5 As shown in the figure, the world coordinate system is OXYZ, with the center line of the cone as the positive direction of the Z axis, the fixed point of the cone as the origin of the coordinate system, and the X3Y3 axis can be established as O3X3Y3Z3 based on the XY axis obtained by rotating the world coordinate system to the coincidence of the Z axes of the two coordinate systems. The position constraints in the three directions of the corresponding Cartesian space can be obtained, and all constraints can be obtained by combining the velocity and acceleration. The conical constraint space generally corresponds to the pelvis, and the conical constraint space is relatively stationary with the pelvis.

[0116] The plane constraint space constraint plane normal vector direction is the free area, and the other side is the forbidden area. Figure 5 As shown, a coordinate system O1X1Y1Z1 is established on the constraint plane, where Z1 is the normal vector of the constraint plane, and X1Y1 can be obtained by rotating the world coordinate system to the XY axis where the Z axes of the two coordinate systems coincide. The plane constraint space generally corresponds to the knee joint, and the constraint relationship corresponding to the plane constraint space is shown in formula (9):

[0117]

[0118] Among them, x3 represents the Z-axis coordinate in the coordinate system O1X1Y1Z1.

[0119] The cuboid constraint space, such as Figure 5 As shown, a coordinate system O2X2Y2Z2 is established based on the edges of the cuboid, where the origin of the coordinate system is at the center of the cuboid. The corresponding constraints are shown in equation (1), and the upper and lower limits of the Cartesian space position are determined based on the boundaries of the cuboid.

[0120] It should be noted that the steps shown in the above process or the flowchart in the accompanying drawings can be executed in a computer system such as a set of computer executable instructions, and although a logical order is shown in the flowchart, in some cases, the steps shown or described can be executed in an order different from that shown here.

[0121] A robot control device is also provided in the present application, and the device is used to implement the above-mentioned embodiments and preferred embodiments, and the descriptions that have been made will not be repeated. The terms "module", "unit", "subunit", etc. used below can implement a combination of software and / or hardware for a predetermined function. Although the devices described in the following embodiments are preferably implemented in software, the implementation of hardware, or a combination of software and hardware, is also possible and conceivable.

[0122] Figure 6 is a structural block diagram of a robot control device according to an embodiment of the present application, such as Figure 6 As shown, the device comprises:

[0123] A first determination module 610 is used to determine the joint acceleration of the robot in the next control cycle according to the joint torque of the robot in the current control cycle, the robot motion information and the predetermined acceleration constraint of the robot in the next control cycle; the acceleration constraint is determined according to the operation space constraint of the robot and the joint space constraint of the robot, and the operation space constraint and the joint space constraint are determined according to the constraint space of the robot;

[0124] A second determination module 620, configured to determine the joint torque of the robot in the next control cycle according to the joint acceleration of the robot in the next control cycle;

[0125] The execution module 630 is used to control the robot according to the joint torque of the robot in the next control cycle.

[0126] It should be noted that the above modules can be functional modules or program modules, and can be implemented by software or hardware. For modules implemented by hardware, the above modules can be located in the same processor; or the above modules can be located in different processors in any combination.

[0127] The present application also provides an electronic device, including a memory and a processor, wherein a computer program is stored in the memory, and the processor is configured to run the computer program to execute the steps in any one of the above method embodiments.

[0128] Optionally, the electronic device may further include a transmission device and an input / output device, wherein the transmission device is connected to the processor, and the input / output device is connected to the processor.

[0129] Optionally, in this embodiment, the processor may be configured to perform the following steps through a computer program:

[0130] S1, determining the joint acceleration of the robot in the next control cycle according to the joint torque of the robot in the current control cycle, the robot motion information and the predetermined acceleration constraint of the robot in the next control cycle; the acceleration constraint is determined according to the operation space constraint of the robot and the joint space constraint of the robot, and the operation space constraint and the joint space constraint are determined according to the constraint space of the robot;

[0131] S2, determining the joint torque of the robot in the next control cycle according to the joint acceleration of the robot in the next control cycle;

[0132] S3, controlling the robot according to the joint torque of the robot in the next control cycle.

[0133] It should be noted that the specific examples in this embodiment can refer to the examples described in the above embodiments and optional implementation modes, and will not be repeated in this embodiment.

[0134] In addition, in combination with a robot control method provided in the above embodiment, a storage medium may be provided in this embodiment to implement the method. The storage medium stores a computer program; when the computer program is executed by a processor, the steps of any robot control method in the above embodiment are implemented.

[0135] It should be understood that the specific embodiments described herein are only used to explain the application, rather than to limit it. Based on the embodiments provided in this application, all other embodiments obtained by ordinary technicians in this field without creative work are within the protection scope of this application.

[0136] Obviously, the drawings are only some examples or embodiments of the present application. For ordinary technicians in the field, the present application can also be applied to other similar situations based on these drawings without creative work. In addition, it is understandable that although the work done in this development process may be complicated and lengthy, for ordinary technicians in the field, certain changes in design, manufacturing or production based on the technical content disclosed in this application are only conventional technical means and should not be regarded as insufficient content disclosed in this application.

[0137] The term "embodiment" in this application refers to a specific feature, structure or characteristic described in conjunction with the embodiment that can be included in at least one embodiment of the present application. The appearance of this phrase in various locations in the specification does not necessarily mean the same embodiment, nor does it mean that it is mutually exclusive with other embodiments and is independent or optional. It is clearly or implicitly understood by those of ordinary skill in the art that the embodiments described in this application can be combined with other embodiments without conflict.

[0138] The above-mentioned embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of patent protection. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the scope of protection of the present application. Therefore, the scope of protection of the present application shall be subject to the attached claims.

Claims

1. A robot control method, characterized in that: The method comprises: Determine the joint acceleration of the robot in the next control cycle according to the joint torque of the robot in the current control cycle, the robot motion information and the predetermined acceleration constraint of the robot in the next control cycle; the acceleration constraint is determined according to the operation space constraint of the robot and the joint space constraint of the robot, and the operation space constraint and the joint space constraint are determined according to the constraint space of the robot; Determining the joint torque of the robot in the next control cycle according to the joint acceleration of the robot in the next control cycle; The robot is controlled according to the joint torque of the robot in the next control cycle.

2. The robot control method according to claim 1, characterized in that: The method further comprises: The acceleration constraint condition of the robot in the next control cycle is determined according to the operation space constraint of the robot, the joint space constraint of the robot, and the relationship between the robot motion information of the current control cycle and the robot motion information of the next control cycle.

3. The robot control method according to claim 2, characterized in that: The robot motion information includes joint positions and joint velocities; Determining the joint acceleration of the robot in the next control cycle according to the joint torque of the robot in the current control cycle, the robot motion information and the predetermined acceleration constraint condition of the robot in the next control cycle includes: Determine the joint acceleration and terminal acceleration of the robot in the current control cycle according to the joint torque, joint position and joint velocity of the robot in the current control cycle; The joint acceleration of the robot in the next control cycle is determined according to the joint acceleration of the robot in the current control cycle, the terminal acceleration and the predetermined acceleration constraint condition of the robot in the next control cycle.

4. The robot control method according to claim 3, characterized in that: Determining the joint acceleration and terminal acceleration of the robot in the current control cycle according to the joint torque, joint position and joint speed of the robot in the current control cycle includes: Calculating the joint acceleration of the robot in the current control cycle using the dynamics model of the robot according to the joint torque, joint external force, joint position and joint velocity of the robot in the current control cycle; Obtain the mapping relationship between the robot's joint acceleration and terminal acceleration; The terminal acceleration of the robot in the current control cycle is calculated according to the joint acceleration of the robot in the current control cycle and the mapping relationship.

5. The robot control method according to claim 3, characterized in that: Determining the joint acceleration of the robot in the next control cycle according to the joint acceleration of the robot in the current control cycle, the terminal acceleration and the predetermined acceleration constraint condition of the robot in the next control cycle includes: Determining the terminal acceleration of the robot in the next control cycle according to the terminal acceleration of the robot in the current control cycle and the predetermined acceleration constraint condition of the robot in the next control cycle; The joint acceleration of the robot in the next control cycle is determined according to the terminal acceleration of the robot in the next control cycle.

6. The robot control method according to claim 1, characterized in that: Determining the joint torque of the robot in the next control cycle according to the joint acceleration of the robot in the next control cycle includes: The joint torque of the robot in the next control cycle is determined according to the joint position and joint velocity of the robot in the current control cycle and the joint acceleration of the robot in the next control cycle.

7. The robot control method according to claim 2 or claim 4, characterized in that: Determining the joint acceleration of the robot in the next control cycle according to the joint acceleration of the robot in the current control cycle, the terminal acceleration and the predetermined acceleration constraint condition of the robot in the next control cycle includes: Determining the terminal acceleration of the robot in the next control cycle according to the terminal acceleration of the robot in the current control cycle and the predetermined acceleration constraint condition of the robot in the next control cycle; According to the terminal acceleration of the robot in the next control cycle, using the dynamic model of the robot, determining the initial joint acceleration of the robot in the next control cycle; The joint acceleration of the robot in the next control cycle is determined according to the initial joint acceleration of the robot in the next control cycle and the predetermined acceleration constraint condition of the robot in the next control cycle.

8. The robot control method according to any one of claims 2 to 6, characterized in that: The geometric shape of the constraint space of the robot is determined according to the surgical area and the target tissue corresponding to the surgical area.

9. A robot control device, characterized in that: The device comprises: A first determination module is used to determine the joint acceleration of the robot in the next control cycle according to the joint torque of the robot in the current control cycle, the robot motion information and the predetermined acceleration constraint of the robot in the next control cycle; the acceleration constraint is determined according to the operation space constraint of the robot and the joint space constraint of the robot, and the operation space constraint and the joint space constraint are determined according to the constraint space of the robot; A second determination module, used to determine the joint torque of the robot in the next control cycle according to the joint acceleration of the robot in the next control cycle; An execution module is used to control the robot according to the joint torque of the robot in the next control cycle.

10. A robot system, characterized in that: The system includes a robot, the robot includes a robotic arm, and the robot is used to execute the robot control method according to any one of claims 1 to 8, and the motion of the robotic arm of the robot is controlled via the method.

11. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the robot control method according to any one of claims 1 to 8 are implemented.

Images