Robot device and control method thereof

Abstract

The present invention provides a robotic device and a control method thereof. [Solution] The robot device includes an execution device and a driving device. The method includes: acquiring at least one inertial sensing data from at least one inertial sensor provided in the execution device and / or the driving device; performing data fusion based on the at least one inertial sensing data to obtain fusion data of the robot device; determining an operating state of the robot device based on the fusion data, and controlling the robot device according to the operating state. According to the present invention, by detecting the inertial sensing data with the inertial sensor and performing data fusion and analysis, it is possible to realize higher detection accuracy of the robot device, improve the reliability of the robot device, eliminate multiple sets of single-axis angle sensors, greatly simplify the structure of the robot device, reduce the weight and volume of the robot device, and save costs.

Description

[Technical field]

[0001] The present invention relates to the field of robotics, and more particularly to a robotic device and a control method thereof. [Background technology]

[0002] With the development of robot technology, the application fields of robot devices, such as the medical field, are becoming wider and wider. In the conventional medical robot device, the carrying of the execution tool such as a surgical instrument by the robot arm can be realized by a parallelogram mechanical transmission structure (PLM), so that the surgical instrument is moved around a fixed remote moving center (RCM). The angle synchronism between the surgical instrument linear drive unit (ISA) and the compact motor module (CMD) is measured by a plurality of single-axis angle sensor modules (ENC) arranged inside and outside the parallelogram mechanical transmission structure. However, the single-axis angle sensor module system can only realize the detection of the pitch angle synchronism between the compact motor module and the surgical instrument linear drive unit, and cannot measure the deformation of the parallelogram mechanical transmission structure in other directions. In addition, the use of the single-axis angle sensor module system increases the number of circuit module devices in the parallelogram mechanical transmission structure, which reduces the reliability of the robot device, increases the complexity of the wiring, and increases the complexity of the structural design, affecting the structural optimization design of the robot device. Summary of the Invention [Problem to be solved by the invention]

[0003] An embodiment of the present invention provides a robot device, a control method, a system, and a computer storage medium for solving at least one of the problems of being unable to detect deformation in other directions of the above-mentioned parallelogram mechanical transmission structure, the complexity of the structure, and the reduced reliability of the robot device. [Means for solving the problem]

[0004] According to a first aspect of the present invention, there is provided a method for controlling a robotic device, the robotic device comprising an execution device for fixing an execution tool and a drive device for driving the execution device to move on the drive device, the method including: acquiring at least one inertial sensing data from at least one inertial sensor provided on the execution device and / or the drive device; performing data fusion based on the at least one inertial sensing data to obtain fusion data of the robotic device; determining an operating state of the robotic device based on the fusion data; and controlling the robotic device according to the operating state.

[0005] Optionally, performing data fusion based on the at least one inertial sensing data to obtain fusion data of the robotic device includes calculating at least one of an actual pitch angle, an actual roll angle, or an actual yaw angle of the drive unit based on the at least one inertial sensing data, and determining a motion state of the robotic device based on the fusion data and controlling the robotic device in accordance with the motion state includes determining whether at least one of the actual pitch angle, the actual roll angle, or the actual yaw angle is synchronized with a corresponding target angle, determining that motion of the robotic device is abnormal if at least one of the actual pitch angle, the actual roll angle, or the actual yaw angle is not synchronized with the corresponding target angle, and controlling the robotic device to stop motion if motion of the robotic device is determined to be abnormal.

[0006] Optionally, determining whether at least one of the real pitch angle, the real roll angle, or the real yaw angle is synchronized to a corresponding target angle includes determining whether at least one of the real pitch angle, the real roll angle, or the real yaw angle is within a corresponding preset angle range that includes the corresponding target angle, and determining that the real pitch angle, the real roll angle, or the real yaw angle is not synchronized to the corresponding target angle if the at least one of the real pitch angle, the real roll angle, or the real yaw angle is not within the corresponding preset angle range.

[0007] Optionally, determining an operating state of the robotic device based on the fusion data and controlling the robotic device according to the operating state further includes: calculating a synchronization rate corresponding to at least one of the real pitch angle, the real roll angle, or the real yaw angle based on at least one of the real pitch angle, the real roll angle, or the real yaw angle and the corresponding target angle; determining whether the synchronization rate corresponding to the real pitch angle, the real roll angle, or the real yaw angle has reached a corresponding synchronization rate threshold; and determining that an operation of the robotic device is abnormal and controlling the operation of the robotic device to stop if the synchronization rate of the real pitch angle, the real roll angle, or the real yaw angle has not reached a corresponding synchronization rate threshold.

[0008] Optionally, determining an operating state of the robotic device based on the fusion data and controlling the robotic device in accordance with the operating state further includes: calculating, based on a synchronization rate corresponding to at least one of the real pitch angle, the real roll angle, or the real yaw angle, whether a change in the synchronization rate of the real pitch angle, the real roll angle, or the real yaw angle during a first preset time period exceeds a corresponding change threshold; and determining that an operation of the robotic device is abnormal and controlling the operation of the robotic device to stop when the change in the synchronization rate of the real pitch angle, the real roll angle, or the real yaw angle during the first preset time period exceeds a corresponding change threshold.

[0009] Optionally, the robotic device further comprises a transmission device connected to the drive device and for moving the drive device, and performing data fusion based on the at least one inertial sensing data, obtaining fusion data of the robotic device includes calculating an output angle of the transmission device based on the at least one inertial sensing data, calculating an error between the output angle of the transmission device and an input angle of the transmission device for each use, and predicting a permanent consumption of the transmission device based on a variation and / or a variation rate of adjacent errors and a mathematical motion model of the transmission device, and determining an operating state of the robotic device based on the fusion data includes determining whether the robotic device is operating normally and a remaining usage time or remaining number of uses of the transmission device based on the permanent consumption.

[0010] Optionally, the robotic device further includes a transmission device connected to the drive device for moving the drive device, and a power device connected to the transmission device for powering the transmission device, and the at least one inertial sensing data includes acceleration data, and performing data fusion based on the at least one inertial sensing data to obtain fusion data of the robotic device includes acquiring actual current data of the power device and predicting theoretical acceleration data of the transmission device based on the actual current data and a mathematical motion model of the transmission device, and determining an operating state of the robotic device based on the fusion data and controlling the robotic device in accordance with the operating state includes determining an occurrence of a collision of the robotic device based on the theoretical acceleration data and the acceleration data, and controlling the operation of the robotic device to stop when it is determined that the robotic device has collided.

[0011] Optionally, the robotic device further includes a transmission device connected to the driving device for moving the driving device, a power device connected to the transmission device for powering the transmission device, and an adjustment device connected to the power device for adjusting an angle of the power device, wherein performing data fusion based on the at least one inertial sensing data to obtain fusion data of the robotic device includes calculating distortion data of the transmission device based on the at least one inertial sensing data and calculating a magnitude and direction of a user's thrust against the driving device based on the distortion data, and determining a motion state of the robotic device based on the fusion data and controlling the robotic device according to the motion state includes controlling an angle of the adjustment device based on the magnitude and direction of the thrust.

[0012] Optionally, performing data fusion based on the at least one inertial sensing data to obtain fusion data of the robotic device includes obtaining a control signal for the robotic device, and data fusing the at least one inertial sensing data and the control signal to obtain the fusion data.

[0013] Optionally, data fusion is performed based on the at least one inertial sensing data to obtain fusion data of the robotic device using a complementary algorithm, a noise model based algorithm, or a particle swarm based algorithm.

[0014] Optionally, the inertial sensing data includes at least one of acceleration data, angular velocity data, or orientation data.

[0015] According to a second aspect of the present invention, a robotic device is provided, comprising: an execution device for fixing an execution tool; a drive device connected to the execution device for driving the execution device; at least one inertial sensor provided on the execution device and / or the drive device for collecting inertial sensing data of the execution device and / or the drive device; and a control device connected to the at least one inertial sensor for performing data fusion based on the at least one inertial sensing data to obtain fusion data of the robotic device; and a control device for determining an operating state of the robotic device based on the fusion data and controlling the robotic device in accordance with the operating state.

[0016] Optionally, the inertial sensor includes at least two of a gyroscope, an accelerometer, or a magnetometer.

[0017] Optionally, the robotic device further comprises a transmission device connected to the drive device for moving the drive device, a power device connected to the transmission device for powering the transmission device, and an adjustment device connected to the power device for adjusting the position and / or orientation of the transmission device and the entire power device. Effect of the Invention

[0018] According to the robot device and the control method thereof of the embodiment of the present invention, by detecting inertial sensing data using an inertial sensor and performing data fusion and analysis, it is possible to achieve higher detection accuracy for the robot device, improve the reliability of the robot device, eliminate multiple sets of single-axis angle sensors, greatly simplify the structure of the robot device, reduce the weight and volume of the robot device, and save costs. [Brief description of the drawings]

[0019] The above and other objects, features and advantages of the present invention will become more apparent by describing the embodiments of the present invention in detail with the drawings. The drawings are for further understanding of the embodiments of the present invention, constitute a part of the specification, and are for interpreting the present invention together with the embodiments of the present invention, but are not intended to limit the present invention. In the drawings, the same reference numerals generally represent the same parts or steps. [Figure 1] FIG. 1 is a schematic diagram showing the principle of a robot device. [Diagram 2] FIG. 2 is a schematic flow chart of pitch angle synchronization detection of a robot device. [Diagram 3] FIG. 3 is a schematic diagram illustrating the principle of a robot device according to an embodiment of the present invention. [Figure 4] FIG. 4 is a schematic flowchart of a control method for a robot device according to an embodiment of the present invention. [Diagram 5] FIG. 5 shows an example of a control method for a robot device according to an embodiment of the present invention. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0020] In order to make the objectives, technical solutions and advantages of the embodiments of the present invention clearer, the exemplary embodiments of the present invention will be described in detail below with reference to the drawings. Obviously, the described embodiments are only some embodiments of the present invention, but not all embodiments. It should be understood that the present invention is not limited to the exemplary embodiments described herein. Based on the embodiments described in the present invention, all other embodiments obtained by those skilled in the art without performing creative labor should fall within the protection scope of the present invention.

[0021] Conventional robotic devices used in the medical field, such as the parallelogram mechanical transmission structure (PLM) of a surgical robot, include one or more pre-tightened metal belts or wire ropes (PSB) for power transmission, and a number of single-axis angle sensor modules (ENC) arranged inside and outside the parallelogram mechanical transmission structure measure the angle synchronism between the surgical instrument linear drive unit (ISA) and the compact motor module (CMD), the metal belt extension value, and the deformation degree of the parallelogram mechanical transmission structure, thereby realizing safety detection of the structural state. However, the single-axis angle sensor module cannot detect the deformation of the parallelogram mechanical transmission structure in other directions or the active collision of the robot arm with the outside world. In addition, the use of the single-axis angle sensor module increases the number of module devices in the parallelogram mechanical transmission structure, which reduces the reliability of the robotic device, increases the complexity of the wiring, and increases the complexity of the structural design, affecting the structural optimization design of the robotic device.

[0022] Referring to FIG. 1, FIG. 1 shows a schematic principle diagram of a robot device. As shown in FIG. 1, a conventional surgical robot realizes that an object is manipulated through several small holes by a remote motion center 1 of a robot arm to improve the safety of surgery. The yaw and pitch motions of a surgical instrument (SGI) 2 at the tip of the robot device are powered by compact motor modules (CMD) 3, 4 installed at the rear end of the robot. Specifically, the compact motor modules 3, 4 transmit power to a surgical instrument linear drive unit (ISA) 10 through a parallelogram mechanical transmission structure (PLM) 5 and a preloaded metal belt mounted inside the parallelogram mechanical transmission structure 5. At the same time, the surgical instrument linear drive unit 10 provides a translational motion along the central axis of the surgical instrument 2, thereby realizing multi-degree-of-freedom control of the surgical instrument.

[0023] The reliability of the power transmission of the robot device shown in FIG. 1 depends on the structural integrity of the parallelogram mechanical transmission structure 5 and the preload of the preload metal belt. The conventional robot device uses a plurality of single-axis angle sensor modules (ENCs) 6, 7, and 8 attached to the parallelogram mechanical transmission structure 5 to measure the change in the input / output angle relationship of each stage of the parallelogram mechanical transmission structure 5 in real time, and checks the integrity of the parallelogram mechanical transmission structure 5 and the preload of the preload metal belt. With reference to FIG. 2, FIG. 2 shows a schematic flow chart of the pitch angle synchronization detection of the robot device. As shown in FIG. 2, the output pitch angle of the compact motor modules 3 and 4 is output to the surgical instrument linear drive unit (ISA) 10 through the parallelogram mechanical transmission structure (PLM) 5 and the preload metal belt attached inside the parallelogram mechanical transmission structure 5. The single-axis angle sensor modules (ENC) 6, 7, 8 feed back the actual pitch angle of the PLM and ISA, and the system state machine processes the actual pitch angle and the output pitch angle of the compact motor modules 3, 4, and then determines whether a pitch angle synchronization error occurs in the parallelogram mechanical transmission structure 5 or whether the preload metal belt breaks, and the controller interrupts the operation and issues an alarm indication for the maintenance of the robot device.

[0024] As shown in Figures 1 and 2, the multiple single-axis angle sensor modules 6, 7, and 8 mounted on the parallelogram mechanical transmission structure 5 need to work together, which increases the circuit design requirements, sensor implementation, and mechanical cooperation of the safety detection system in the parallelogram mechanical transmission structure, complicates the design of the entire system, reduces reliability, reduces the design freedom of the parallelogram mechanical transmission structure, and increases the weight of the machine. In addition, the single-axis angle sensor module system is not equipped to detect yaw angle synchronization abnormalities.

[0025] In consideration of the above, a robot apparatus and a control method thereof according to an embodiment of the present invention have been proposed. Referring to FIG. 3, FIG. 3 shows a schematic principle diagram of a robot apparatus according to an embodiment of the present invention. As shown in FIG. 3, the robot apparatus 300 includes an execution device 310 for fixing an execution tool 311, a drive device 320 connected to the execution device 310 for driving the execution device 310, at least one inertial sensor 330 provided on the execution device 310 and / or the drive device 320 for collecting inertial sensing data of the drive device 320, and a control device 340 connected to the at least one inertial sensor 330 for executing at least some or all of the steps in the control method of a robot apparatus according to an embodiment of the present invention.

[0026] In some embodiments, the execution tool 311 may be a surgical instrument (SGI). In some embodiments, the surgical instrument (SGI) may be a tool for performing a surgical operation (e.g., grasping, shearing, cutting, pinching, suturing, etc.) on a target object, such as surgical forceps, surgical scissors, high-frequency electric scalpel, suture needle, etc. In some embodiments, the surgical instrument (SGI) may be a tool that supports a surgical operation, such as various image acquisition devices, a rearview mirror, etc. Note that the above surgical instruments are merely examples and are not intended to limit the surgical instruments, and the surgical instrument may be anything related to performing surgery and is not limited here.

[0027] In some embodiments, the execution device 310 may be a surgical tool drive module (Motor Pack, MPK) for driving fixed surgical tools to perform various surgical tasks.

[0028] In some embodiments, the driver 320 may be a surgical instrument linear drive unit (Insertion Arm, ISA).

[0029] In some embodiments, the inertial sensor 330 includes at least two of a gyroscope, an accelerometer, or a magnetometer. In some embodiments, the inertial sensor 330 may be two types of a gyroscope and an accelerometer, a gyroscope and a magnetometer, or more of a gyroscope, an accelerometer, and a magnetometer.

[0030] In some embodiments, the controller 340 may be implemented in software, hardware, firmware, or a combination thereof. In some embodiments, the controller 340 may use at least one of a circuit, an Application Specific Integrated Circuit (ASIC), a Digital Signal Processor (DSP), a Digital Signal Processing Device (DSPD), a Programmable Logic Device (PLD), a Field Programmable Gate Array (FPGA), a Central Processing Unit (CPU), a PLC, a microcontroller, and a microprocessor.

[0031] Optionally, the robotic device 300 further includes a transmission device 350 connected to the drive device 320 for moving the drive device 320, a power device 360 ​​connected to the transmission device 350 for powering the transmission device 350, and an adjustment device 370 connected to the power device 360 ​​for adjusting the position and / or orientation of the entire transmission device 350 and the power device 360.

[0032] In some embodiments, the transmission 350 may be a Parallel Mechanism (PLM).

[0033] In some embodiments, the power unit 360 may be one or more motors or may further be a Compact Motor Drive (CMD). In some embodiments, the controller 340 may control the output angle of the power unit 360.

[0034] In some embodiments, the adjustment device 370 may be a set-up joint (SUJ) provided at the rear end of the robot device. As shown in FIG. 3, the robot device may be a surgical robot, and the transmission device 350 and the power device 360 ​​may be a robot arm of the surgical robot. The set-up joint (SUJ) is provided at the rear end of the robot arm and connected to the robot arm, and can perform at least one of horizontal movement, vertical movement, and orientation adjustment of the entire robot arm, thereby adjusting the orientation and position of the robot arm in a suitable range, ensuring that the area where the target object needs to be operated is within the motion range of the execution device 310, and making the distance between the robot arm and the target object suitable for the surgical operation of the target object.

[0035] 4 is a schematic flow chart of a control method for a robotic device according to an embodiment of the present invention. As shown in FIG. 4, the method can be performed by a robotic device, and the control method for a robotic device 400 includes a step 410 of acquiring at least one inertial sensing data from at least one inertial sensor provided in the execution device and / or the drive device, a step 420 of performing data fusion based on the at least one inertial sensing data to obtain fusion data for the robotic device, and a step 430 of determining an operating state of the robotic device based on the fusion data and controlling the robotic device according to the operating state.

[0036] Here, by collecting inertial sensing data by the inertial sensors provided in the execution device and / or the drive device and performing data fusion and analysis, it is possible to realize more accurate detection of the robot device, accurately analyze the operating state of the robot device, and perform corresponding control, thereby improving the reliability of the robot device. Compared with conventional robot devices, multiple sets of single-axis angle sensors are omitted, which greatly simplifies the structure of the robot device, reduces the weight and volume of the robot device, and saves costs.

[0037] In some embodiments, the inertial sensing data includes at least one of acceleration data, angular velocity data, or geomagnetic field direction data. Furthermore, in some embodiments, the acceleration data may be collected by an accelerometer. In some embodiments, the angular velocity data may be collected by a gyroscope. In some embodiments, the geomagnetic field direction data may be collected by a magnetometer. In some embodiments, the accelerometer, the gyroscope, or the magnetometer transmits the collected inertial sensing data to a control device, and the control device performs a data fusion operation to obtain fusion data.

[0038] In some embodiments, performing data fusion based on the at least one inertial sensing data may employ a complementary algorithm, a noise model based algorithm, or a particle swarm based algorithm.

[0039] Here, the data fusion algorithms are all based on optimizing the use of inertial sensing data and control signals, including, but not limited to, performing data fusion on inertial sensing data (e.g., acceleration data, angular velocity data, or direction data), the output of the driving device (e.g., driver torque output), the input current of the power device (e.g., motor detection current), etc. to generate an optimized transmission deformation signal. In contrast, the complementary algorithm has the lowest calculation amount and has a general fusion effect, the algorithm based on the noise model has a somewhat high calculation amount and an excellent fusion effect, and the algorithm based on the particle swarm has the highest calculation amount and the best data fusion effect.

[0040] Optionally, in step S420, performing data fusion based on the at least one inertial sensing data to obtain fusion data of the robotic device includes calculating at least one of an actual pitch angle, an actual roll angle, or an actual yaw angle of the drive unit based on the at least one inertial sensing data, and in step S430, determining a motion state of the robotic device based on the fusion data and controlling the robotic device in accordance with the motion state includes determining whether at least one of the actual pitch angle, the actual roll angle, or the actual yaw angle is synchronized with a corresponding target angle, determining that motion of the robotic device is abnormal if at least one of the actual pitch angle, the actual roll angle, or the actual yaw angle is not synchronized with the corresponding target angle, and controlling the robotic device to stop motion if motion of the robotic device is determined to be abnormal.

[0041] Here, by determining whether the actual pitch angle, the actual roll angle, or the actual yaw angle of the driving device are synchronized with the respective target angles, it is possible to determine whether the operation of the robot device is abnormal. For example, referring to FIG. 3, in a surgical robot, the execution tool 311 may be a surgical instrument (SGI), the execution device 310 may be a surgical instrument driving module (MPK), the driving device 320 may be a surgical instrument linear driving unit (ISA), the surgical instrument driving module MPK may be provided on the trajectory of the surgical instrument linear driving unit ISA, and the surgical instrument linear driving unit ISA drives the surgical instrument driving module MPK to which the surgical instrument SG1 is fixed to move on the trajectory and perform the corresponding surgical operation. In this way, since the pitch angle, the roll angle, or the yaw angle of the execution device 310 and the driving device 320 are the same, at least one inertial sensor can be provided only in the execution device 310 or the driving device 320, and thus data fusion can be performed on the data collected by the at least one inertial sensor to obtain fusion data. It is also possible to simultaneously install at least one inertial sensor in both the execution device 310 and the driving device 320 for simultaneous detection. In this way, redundancy of the inertial sensor can be realized, and the multiple inertial sensing data obtained thereby are also redundant with each other, thereby ensuring the accuracy of the data and improving the accuracy of subsequent data fusion.

[0042] Furthermore, in some embodiments, when multiple inertial sensors are provided, data fusion can be performed using the average or median of multiple inertial sensing data of the same type during data fusion. For example, for angular velocity data of the same type of drive device, the average or median of multiple angular velocity data of the drive device collected by multiple inertial sensors can be used as the angular velocity data of the drive device during data fusion, and other types of data can be treated in the same manner.

[0043] In some embodiments, the motion abnormality includes at least one of a pitch angle, a roll angle, or a yaw angle being out of sync. In some embodiments, the motion abnormality may also include a breakage of one or more pre-tightened metal belts or wire ropes (PSB) in a parallelogram mechanical transmission structure (PLM).

[0044] In some embodiments, the control method 400 may include issuing an alert when the operation of the robotic device is determined to be abnormal. In some embodiments, after issuing the alert, the robotic device may be shut down to stop operation. In this manner, issuing an alert when the operation of the robotic device is abnormal allows a user to perform down maintenance in a timely manner.

[0045] In some embodiments, the alarm may be an acoustic-optical mode alarm or a display on the robotic device may indicate that at least one of the pitch, roll, or yaw angles are out of sync and / or that a metal belt or wire rope (PSB) in the transmission is broken.

[0046] In some embodiments, determining whether at least one of the real pitch angle, the real roll angle, or the real yaw angle is synchronized to a corresponding target angle includes determining whether at least one of the real pitch angle, the real roll angle, or the real yaw angle is within a corresponding preset angle range that includes the corresponding target angle, and determining that the real pitch angle, the real roll angle, or the real yaw angle is not synchronized to the corresponding target angle if the at least one of the real pitch angle, the real roll angle, or the real yaw angle is not within the corresponding preset angle range.

[0047] In some embodiments, the preset angle range may be a range centered around a target angle. For example, t The preset angle range is [A t -a, At +b], where a and b may be the same or different and are all positive numbers. It can be seen that the pitch angle, roll angle, and yaw angle all have corresponding target angles.

[0048] Here, the values ​​of a and b can be set as necessary, and are not limited thereto. For example, a and b may be set in consideration of transient vibrations due to the structural flexibility of the transmission device.

[0049] In some embodiments, the target angle may be an angle output by a powered device, for example, at least one of a pitch angle, a roll angle, and a yaw angle output by a compact motor module (CMD).

[0050] In some embodiments, in step S430, determining a motion state of the robotic device based on the fusion data may also include determining that the actual pitch angle, the actual roll angle, or the actual yaw angle is synchronized with a corresponding target angle if the actual pitch angle, the actual roll angle, or the actual yaw angle is within a corresponding preset angle range.

[0051] In some examples, in step S430, determining a motion state of the robotic device based on the fusion data may also include determining that the actual pitch angle, the actual roll angle, or the actual yaw angle is not synchronized with a corresponding target angle if the actual pitch angle, the actual roll angle, or the actual yaw angle is not within a corresponding preset angle range.

[0052] Furthermore, in some embodiments, the operation of the robotic device is determined to be normal if any of the actual pitch angle, the actual roll angle, or the actual yaw angle is synchronized with the corresponding target angle.

[0053] In some embodiments, in step S430, determining a motion state of the robotic device based on the fusion data and controlling the robotic device according to the motion state further includes: calculating a synchronization rate corresponding to at least one of the real pitch angle, the real roll angle, or the real yaw angle based on at least one of the real pitch angle, the real roll angle, or the real yaw angle and the corresponding target angle; determining whether the synchronization rate corresponding to the real pitch angle, the real roll angle, or the real yaw angle has reached a corresponding synchronization rate threshold; and determining that the motion of the robotic device is abnormal and controlling the motion of the robotic device to stop if the synchronization rate of the real pitch angle, the real roll angle, or the real yaw angle has not reached a corresponding synchronization rate threshold.

[0054] In some embodiments, in step S430, determining an operating state of the robotic device based on the fusion data and controlling the robotic device according to the operating state further includes: calculating, based on a synchronization rate corresponding to at least one of the actual pitch angle, the actual roll angle, or the actual yaw angle, whether a change in a synchronization rate of the actual pitch angle, the actual roll angle, or the actual yaw angle during a first preset time period exceeds a corresponding change threshold; and determining that an operation of the robotic device is abnormal and controlling the operation of the robotic device to stop when the change in the synchronization rate of the actual pitch angle, the actual roll angle, or the actual yaw angle during the first preset time period exceeds a corresponding change threshold.

[0055] Here, by determining the synchronization rate or change in the synchronization rate of the real pitch angle, real roll angle, or real yaw angle of the driving device, it can also be determined whether the robot device is operating normally. If the synchronization rate of the real pitch angle, real roll angle, or real yaw angle does not reach the corresponding synchronization rate threshold, or if the change in the synchronization rate of the real pitch angle, real roll angle, or real yaw angle is too large (exceeds the change threshold), the robot device is in an abnormal operating state, and the operation of the robot device can be controlled to be stopped in order to prevent damage to the robot device.

[0056] In this way, according to the control method for a robot device of the embodiment of the present invention, the roll angle, pitch angle, and yaw angle of the transmission device (e.g., a parallelogram mechanical transmission structure) and the drive device (e.g., a surgical instrument linear drive unit) are simultaneously detected, thereby making it possible to detect the deformation of the robot device in multiple directions, and increasing the integrity of the surgical robot angle synchronization monitoring safety system.

[0057] Optionally, in step S420, performing data fusion based on the at least one inertial sensing data to obtain fusion data of the robotic device includes obtaining a control signal of the robotic device, and data fusing the at least one inertial sensing data and the control signal to obtain the fusion data.

[0058] Here, the control signal of the robot device and the inertial sensing data detected by the inertial sensor are data fused, and the information contained in the obtained fusion data is more comprehensive and more accurate. By analyzing the state of the robot device based on this more comprehensive and more accurate fusion data, the accuracy of the state analysis of the robot device is improved, which is more advantageous for accurate control of the robot device.

[0059] In some embodiments, the control signal of the robotic device may include a power unit output signal (i.e., a transmission input signal) such as an output angle, an output current, an output torque, etc., or a drive unit output signal such as a drive torque output, etc. It should be understood that the control signal may be any signal for controlling a device or apparatus in the robotic device and is not limited herein.

[0060] Optionally, the robotic device further includes a transmission device connected to the driving device and for moving the driving device, and in step S420, performing data fusion based on the at least one inertial sensing data to obtain fusion data of the robotic device includes calculating an output angle of the transmission device based on the at least one inertial sensing data, calculating an error between the output angle of the transmission device and an input angle of the transmission device for each use, and predicting a permanent consumption of the transmission device based on a variation and / or a variation rate of adjacent errors and a mathematical motion model of the transmission device, and in step S430, determining an operating state of the robotic device based on the fusion data includes determining whether the robotic device is operating normally and a remaining usage time or remaining number of uses of the transmission device based on the permanent consumption.

[0061] In the above embodiment, the method may further include obtaining an input angle of the transmission device before calculating an error between an output angle of the transmission device and an input angle of the transmission device for each use.

[0062] Here, the input angle of the transmission is the output angle of the power unit, and the power unit can be controlled by the control device, i.e., the control device can send a corresponding control command to the power unit and control the power unit to output a corresponding angle, so that the output angle of the power unit (i.e., the input angle of the transmission) can be the control signal.

[0063] In some embodiments, determining whether the robotic device is operating normally and the remaining usage time or remaining number of uses of the transmission device based on the permanent consumption amount includes: determining that the operation of the robotic device is abnormal and setting the remaining usage time of the transmission device to 0 if the permanent consumption amount exceeds a preset consumption amount.

[0064] Furthermore, in some embodiments, the operation of the robotic device is determined to be abnormal, and the operation of the robotic device is controlled to stop.

[0065] In some embodiments, determining whether the robotic device is operating normally and the remaining usage time or number of uses of a transmission based on the permanent consumption amount includes determining that the operation of the robotic device is normal if the permanent consumption amount does not exceed a preset consumption amount, and the remaining usage time or number of uses of the transmission is a difference between the permanent consumption amount and the preset consumption amount divided by an average consumption amount.

[0066] In some embodiments, the average consumption can be calculated based on the permanent consumption and the duration or number of uses corresponding to the permanent consumption, for example, the average consumption is the permanent consumption divided by the duration of the corresponding use, or the average consumption is the permanent consumption divided by the number of uses corresponding to the permanent consumption.

[0067] In some embodiments, the preset consumption may be a consumption corresponding to a time when a pre-alert or replacement is required. Here, when the preset consumption represents a time when a pre-alert is required, the calculated remaining use time or number of uses of the transmission indicates a remaining use time or number of uses until the transmission performs the next pre-alert. When the preset consumption represents a time when a replacement is required, the calculated remaining use time or number of uses of the transmission indicates a remaining use time or number of uses until the transmission is replaced. Note that the preset consumption can be set as needed and is not limited here.

[0068] In some embodiments, the powertrain input angles may be the powerplant output angles, for example the pitch, roll, and yaw angles output by a Compact Motor Module (CMD).

[0069] Here, the transmission may be a parallelogram mechanical transmission structure, and the mathematical motion model of the transmission may adopt the mathematical motion model of the parallelogram mechanical transmission structure. The permanent consumption of the transmission may be predicted based on the fluctuation / fluctuation rate of the difference value between the input and output of the transmission and the mathematical motion model of the transmission, thereby obtaining the remaining usage time of the transmission. The permanent consumption may be the irrecoverable consumption in the transmission. Taking the transmission belt in the transmission (PLM) as an example, the permanent consumption refers to the irrecoverable extension length of the transmission belt, and the number of times or time that the transmission belt can be used may be predicted based on the extension value of the transmission belt every time the transmission (PLM) is used.

[0070] In some embodiments, the method 400 for controlling a robotic device may further include reporting at least one of a permanent consumption amount, a remaining usage time, or a remaining number of usages of the transmission device by an output device of the robotic device.

[0071] In some embodiments, the method 400 for controlling a robotic device may further include determining a time required for maintenance of the transmission device depending on at least one of a remaining usage time or a remaining number of usages of the transmission device.

[0072] In some embodiments, the method 400 for controlling a robotic device may further include reporting, via an output device of the robotic device, a time required for maintenance of the transmission.

[0073] In this way, by predicting at least one of the permanent consumption, remaining usage time, or remaining number of uses of the transmission device, maintenance work can be arranged rationally, thereby reducing the number of maintenance operations and costs, and improving maintenance efficiency.

[0074] Optionally, the at least one inertial sensing data includes acceleration data, and in step S420, performing data fusion based on the at least one inertial sensing data to obtain fusion data of the robotic device includes obtaining actual current data of the power unit and predicting theoretical acceleration data of the transmission device based on the actual current data and a mathematical motion model of the transmission device, and in step S430, determining an operating state of the robotic device based on the fusion data and controlling the robotic device according to the operating state includes determining whether a collision has occurred for the robotic device based on the theoretical acceleration data and the acceleration data, and controlling the operation of the robotic device to stop when it is determined that the robotic device has collided.

[0075] In some embodiments, determining whether a collision has occurred for the robotic device based on the theoretical acceleration data and the acceleration data may include determining that the robotic device has collided if the theoretical acceleration data is equal to or greater than the acceleration data, and determining that a collision has not occurred for the robotic device if the theoretical acceleration data is smaller than the acceleration data.

[0076] Here, the power unit may be a joint of a robot arm of a surgical robot, and the transmission unit may be a robot arm, and the acceleration data collected by the inertial sensor can be compared with the theoretical acceleration output by the transmission unit, and when a collision occurs, the power unit connected to the transmission unit increases its input current to maintain the original motion speed, and at this time, the theoretical acceleration data obtained from the detected input current and the mathematical motion model of the transmission unit increases, but since the acceleration data actually detected by the inertial sensor becomes smaller due to the collision, when a collision occurs, the theoretical acceleration data becomes larger than the detected acceleration data. As a result, it is possible to actively detect whether or not the transmission unit (e.g., a robot arm) is colliding with a surrounding object, which mainly includes collisions between robot arms and collisions between the robot arm and the outside world, thereby improving the safety of the robot device.

[0077] In some embodiments, the control method 400 may further include providing a notification to a user when the occurrence of a collision is detected, for example, by emitting a particular sound or voice, or by displaying a notification message on a display device of the robotic device, such as an output device of the robotic device, to indicate the occurrence of the collision.

[0078] In some embodiments, the control method 400 may further include stopping the robot arm where the collision occurred after the occurrence of the collision is detected, so that the user can easily remove the obstacle.

[0079] Optionally, in step S420, performing data fusion based on the at least one inertial sensing data to obtain fusion data of the robotic device includes calculating distortion data of the transmission device based on the at least one inertial sensing data and calculating a magnitude and direction of a user's thrust on the drive device based on the distortion data, and in step S430, determining a motion state of the robotic device based on the fusion data and controlling the robotic device according to the motion state includes controlling an angle of the adjustment device based on the magnitude and direction of the thrust.

[0080] Specifically, referring to FIG. 3, when the surgical instrument is attached to the robot device, the transmission device 350 (e.g., parallelogram mechanical transmission structure) and the power device 360 ​​(e.g., compact motor module) are in an angle maintaining state. When a user (e.g., a medical professional) pushes the execution device 310 (e.g., a linear drive unit of a surgical instrument), the transmission device 350 (e.g., parallelogram mechanical transmission structure) is deformed, and the magnitude and direction of the user's thrust can be estimated from the deformation degree of the transmission device 350 (e.g., parallelogram mechanical transmission structure) detected in real time by at least one inertial sensor 330. The control device obtains the required torque and speed output of each joint of the adjustment device 370 (e.g., adjustment arm (SUJ)) located at the rear end of the robot device through an inverse kinematic algorithm, and sends a corresponding command to the adjustment arm to realize the angle control of the adjustment arm, allowing the user to easily move the adjustment arm.

[0081] An example of a control method for a robot device according to an embodiment of the present invention is shown in Fig. 5. As shown in Fig. 5, a control method 500 for a robot device includes the following steps.

[0082] In step S510, the pitch angle and yaw angle output by the power unit (e.g., compact motor module CMD) are output to a drive unit (e.g., surgical instrument linear drive unit ISA) via a transmission unit (e.g., parallelogram mechanical transmission structure PLM) and a preloaded metal belt attached inside the transmission unit.

[0083] In step S520, at least one inertial sensor IMU provided in the driving device collects acceleration data, angular velocity data and direction data of the driving device, and obtains the actual pitch angle, actual roll angle and actual yaw angle of the driving device.

[0084] In step S530, a data fusion algorithm is combined to perform data fusion between the collected data of each sensor and the control signal and to analyze the operating state of the robot device.

[0085] In step S541, it is determined whether at least one of the actual pitch angle, the actual roll angle, and the actual yaw angle is synchronized with the corresponding target angle. If at least one of the pitch angle, the roll angle, and the yaw angle is not synchronized, it is determined that the operation of the robot device is abnormal, and an alarm is issued.

[0086] In step S542, the system is shut down to await maintenance.

[0087] In step S551, the permanent consumption and / or remaining usage time of the transmission is predicted based on the difference between the input and output of the transmission and a mathematical model of the transmission, and the time required for maintenance of the transmission is determined according to the permanent consumption and / or remaining usage time of the transmission.

[0088] In step S552, the time required for maintenance of the transmission device is notified via the output device of the mobile phone device.

[0089] In step S561, the deformation degree of a transmission device (eg, a parallelogram mechanical transmission structure) is calculated based on the inertial sensing data detected in real time by at least one inertial sensor, and the magnitude and direction of the user's thrust are estimated.

[0090] In step S562, send a corresponding command to the adjustable arm according to the magnitude and direction of the thrust, so as to realize the angle control of the adjustable arm.

[0091] In step S571, it is determined whether the transmission has collided with a surrounding object based on the fusion data and the acceleration data.

[0092] In step S572, if a collision is detected, the arm where the collision occurred is temporarily stopped until medical personnel remove the obstacle.

[0093] As described above, the robot device and control method thereof according to the embodiment of the present invention detects inertial sensing data using an inertial sensor, and performs data fusion and analysis, thereby achieving higher detection accuracy for the robot device, improving the reliability of the robot device, eliminating the need for multiple sets of single-axis angle sensors, greatly simplifying the structure of the robot device, reducing the weight and volume of the robot device, and saving costs.

[0094] It is obvious to those skilled in the art that each example unit and algorithm step described in connection with the embodiments disclosed herein can be realized by electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed by hardware or software depends on the specific application and design constraints of the technical solution. A professional engineer can realize the described functions using different methods for each specific application, and such realization does not depart from the scope of the present invention.

[0095] In some embodiments provided by the present application, it should be understood that the disclosed apparatus and method may be realized in other ways. For example, the above-mentioned apparatus embodiments are merely exemplary, for example, the division of the above units is merely a logical functional division, and in actual implementation, there may be other divisions, such as multiple units or components may be combined or integrated into another apparatus, or some features may be ignored or not implemented.

[0096] It should be understood that although numerous specific details are described herein, embodiments of the present invention may be practiced without these specific details, and in some instances, well-known methods, structures and techniques have not been shown in detail in order not to obscure an understanding of this specification.

[0097] Similarly, in the description of each exemplary embodiment of the present invention, in order to succinctly summarize the present invention and facilitate understanding of one or more aspects, it should be understood that each feature of the present invention may be grouped together from a single embodiment, drawing or description. However, the method of the present invention should not be interpreted as reflecting more features of the invention for which protection is sought than the features set forth in each claim. Rather, as reflected in the corresponding claim, the point of the invention is that the corresponding technical problem can be solved using fewer than all features of a single embodiment in a disclosure. Therefore, the claims according to the specific embodiment are expressly incorporated into the specific embodiment, with each claim being itself a separate embodiment of the present invention.

[0098] Those skilled in the art will understand that, apart from feature repulsions, all features disclosed in this specification (including the claims, abstract and drawings), and all steps or units of any method or apparatus so disclosed, may be combined in any combination. Each feature disclosed in this specification (including the claims, abstract and drawings), unless expressly stated otherwise, may be replaced by alternative features serving the same, equivalent or similar purpose.

[0099] In addition, those skilled in the art will understand that some of the embodiments described herein include some features that are included in other embodiments but not in other features, and that a combination of features from different embodiments is also within the scope of the present invention and constitutes a different embodiment. For example, any of the embodiments for which protection is sought in the claims can be used in any combination.

[0100] The above examples illustrate the present invention rather than limit it, and it should be noted that a person skilled in the art could design alternative embodiments without departing from the scope of the appended claims. In the claims, any reference numbers placed in parentheses shall not be constructed as a limitation of the claims. The word "comprises" does not exclude the presence of parts or steps not listed in the claims. The word "a" or "one" preceding a part does not exclude the presence of a plurality of such parts. The present invention may be realized by hardware comprising several different elements and by a suitably programmed computer. In a unit claim enumerating several devices, several of these devices may be embodied by the same hardware. The use of the words first, second and third does not indicate any order and may be interpreted as names of these words.

[0101] The above is merely a specific embodiment or description of the specific embodiment of the present invention, and the protection scope of the present invention is not limited thereto, and any modifications or replacements that a person skilled in the art can easily think of within the protection scope of the present invention should be included in the protection scope of the present invention. The protection scope of the present invention should be in accordance with the protection scope of the claims.

Claims

1. A method for controlling a robot device, comprising: The robot device includes an execution device for fixing an execution tool, and a driving device for driving the execution device to move on the driving device; The method for controlling the robot device includes: acquiring a plurality of inertial sensing data from a plurality of inertial sensors provided in the execution device and / or the drive device; performing data fusion based on the plurality of inertial sensing data to obtain fusion data of the robot device; determining an operating state of the robotic device based on the fusion data, and controlling the robotic device in accordance with the operating state; Determining an operating state of the robotic device based on the fusion data and controlling the robotic device in accordance with the operating state Calculating at least one of an actual pitch angle, an actual roll angle, or an actual yaw angle of the drive unit based on the fusion data; determining whether at least one of the actual pitch angle, the actual roll angle, or the actual yaw angle is within a corresponding preset angle range; determining that a movement of the robotic device is abnormal when at least one of the actual pitch angle, the actual roll angle, or the actual yaw angle is not within the corresponding preset angle range; and when it is determined that the operation of the robotic device is abnormal, controlling the operation of the robotic device to stop. When the inertial sensor is provided in the execution device, the drive device drives the execution device to move in a straight line. A method for controlling a robot device comprising:

2. A method for controlling a robot device, comprising: The robot device includes an execution device for fixing an execution tool, and a driving device for driving the execution device to move on the driving device; The method for controlling the robot device includes: acquiring a plurality of inertial sensing data from a plurality of inertial sensors provided in the execution device and / or the drive device; performing data fusion based on the plurality of inertial sensing data to obtain fusion data of the robot device; determining an operating state of the robotic device based on the fusion data, and controlling the robotic device in accordance with the operating state; Determining an operating state of the robotic device based on the fusion data and controlling the robotic device in accordance with the operating state Calculating at least one of an actual pitch angle, an actual roll angle, or an actual yaw angle of the drive unit based on the fusion data; Calculating one or more degrees of agreement between at least one of an actual pitch angle, an actual roll angle, or an actual yaw angle and a corresponding target angle based on at least one of the actual pitch angle, the actual roll angle, or the actual yaw angle, and the corresponding target angle; determining whether a match corresponding to the actual pitch angle, the actual roll angle, or the actual yaw angle has reached a corresponding match threshold; determining that an operation of the robotic device is abnormal when a degree of agreement corresponding to the actual pitch angle, the actual roll angle, or the actual yaw angle does not reach a corresponding degree of agreement threshold, and controlling the operation of the robotic device to stop; When the inertial sensor is provided in the execution device, the drive device drives the execution device to move in a straight line. A method for controlling a robot device comprising:

3. A method for controlling a robot device, comprising: The robot device includes an execution device for fixing an execution tool, and a driving device for driving the execution device to move on the driving device; The method for controlling the robot device includes: acquiring a plurality of inertial sensing data from a plurality of inertial sensors provided in the execution device and / or the drive device; performing data fusion based on the plurality of inertial sensing data to obtain fusion data of the robot device; determining an operating state of the robotic device based on the fusion data, and controlling the robotic device in accordance with the operating state; Determining an operating state of the robotic device based on the fusion data and controlling the robotic device in accordance with the operating state Calculating at least one of an actual pitch angle, an actual roll angle, or an actual yaw angle of the drive unit based on the fusion data; Calculating one or more degrees of agreement between at least one of an actual pitch angle, an actual roll angle, or an actual yaw angle and a corresponding target angle based on at least one of the actual pitch angle, the actual roll angle, or the actual yaw angle, and the corresponding target angle; Calculating whether a change in the degree of agreement of the actual pitch angle, the actual roll angle, or the actual yaw angle during a first preset time period exceeds a corresponding change threshold value based on a degree of agreement corresponding to at least one of the actual pitch angle, the actual roll angle, or the actual yaw angle; determining that an operation of the robotic device is abnormal when a change in the coincidence of the actual pitch angle, the actual roll angle, or the actual yaw angle during the first preset time period exceeds a corresponding change threshold, and controlling the operation of the robotic device to stop; When the inertial sensor is provided in the execution device, the drive device drives the execution device to move in a straight line.

4. A method for controlling a robot device according to claim 3.

4. A method for controlling a robot device, comprising: The robot device includes an execution device for fixing an execution tool, a drive device for driving the execution device to move on the drive device, and a transmission device connected to the drive device for moving the drive device, The method for controlling the robot device includes: acquiring a plurality of inertial sensing data from a plurality of inertial sensors provided in the execution device and / or the drive device; performing data fusion based on the plurality of inertial sensing data to obtain fusion data of the robot device; determining an operating state of the robotic device based on the fusion data, and controlling the robotic device in accordance with the operating state; Determining an operating state of the robotic device based on the fusion data and controlling the robotic device in accordance with the operating state Calculating an output angle of the transmission based on the fusion data; Calculating an error between an output angle of the transmission device and an input angle of the transmission device for each use; - predicting a permanent consumption of the transmission based on the variation and / or the rate of variation of the error and on a mathematical motion model of the transmission; determining whether the robot device is operating normally and the remaining usage time or the remaining number of usages of the transmission device based on the permanent consumption amount; When the inertial sensor is provided in the execution device, the drive device drives the execution device to move in a straight line. A method for controlling a robot device comprising:

5. the robot device further includes a transmission device connected to the drive device for moving the drive device, and a power device connected to the transmission device for providing power to the transmission device; The plurality of inertial sensing data includes acceleration data, Obtaining actual current data of the power plant; predicting theoretical acceleration data of the transmission based on the actual current data and a mathematical motion model of the transmission; Determining an operating state of the robotic device based on the fusion data and controlling the robotic device in accordance with the operating state determining whether a collision has occurred for the robotic device based on a comparison between the theoretical acceleration data and the acceleration data; and and when it is determined that the robotic device has collided, controlling the robotic device to stop its operation.

2. The method of claim 1 .

6. A method for controlling a robotic device, comprising: The robot device includes an execution device for fixing an execution tool, a drive device for driving the execution device to move on the drive device, a transmission device connected to the drive device and for moving the drive device, a power device connected to the transmission device and for providing power to the transmission device, and an adjustment device connected to the power device and for adjusting a position and / or an orientation of the power device, The method for controlling the robot device includes: acquiring a plurality of inertial sensing data from a plurality of inertial sensors provided in the execution device and / or the drive device; performing data fusion based on the plurality of inertial sensing data to obtain fusion data of the robot device; determining an operating state of the robotic device based on the fusion data, and controlling the robotic device in accordance with the operating state; Performing data fusion based on the plurality of inertial sensing data to obtain fusion data of the robot device, Calculating distortion data of the transmission device based on the plurality of inertial sensing data; Calculating a magnitude and a direction of a thrust force of a user against the drive device as the fusion data based on the distortion data; Determining an operating state of the robotic device based on the fusion data and controlling the robotic device in accordance with the operating state controlling at least one angle of the adjustment device based on a magnitude and a direction of the thrust; When the inertial sensor is provided in the execution device, the drive device drives the execution device to move in a straight line. A method for controlling a robot device comprising:

7. Performing data fusion based on the plurality of inertial sensing data to obtain fusion data of the robot device, obtaining a control signal for the robotic device; Data fusing the plurality of inertial sensing data and the control signal to obtain the fusion data. The method according to any one of claims 1 to 6.

8. performing data fusion based on the plurality of inertial sensing data to obtain fusion data of the robotic device, using a complementary algorithm, an algorithm based on a noise model, or an algorithm based on a particle swarm; The method according to any one of claims 1 to 6.

9. The inertial sensing data includes at least one of acceleration data, angular velocity data, or orientation data. The method according to any one of claims 1 to 6.

10. A robotic device (300), an execution device (310) for fixing an execution tool (311); a drive unit (320) connected to the execution unit (310) for driving the execution unit (310); a plurality of inertial sensors (330) provided on the execution device (310) and / or the drive device (320) for collecting inertial sensing data of the execution device (310) and / or the drive device (320); a control device (340) connected to said plurality of inertial sensors (330) for carrying out the steps of the control method according to any one of claims 1 to 6; Equipped with A robot device comprising:

11. the inertial sensor includes at least two of a gyroscope, an accelerometer, or a magnetometer; 11. The robot device according to claim 10.

12. a transmission device connected to the drive device for moving the drive device; a power unit connected to the transmission for powering the transmission; and an adjustment device connected to the power unit for adjusting the position and / or orientation of the transmission device and the entire power unit.

11. The robot device according to claim 10.

Images