A method, apparatus, and controller for motion control of a robotic instrument

Abstract

Embodiments of the present application disclose a motion control method, device and controller of a robot instrument. The method can comprise: obtaining a desired motion angle of a target instrument in a current period and an actual motion angle of the target instrument in a previous period of the current period, and determining a desired motion speed of the target instrument in the current period according to the desired motion angle and the actual motion angle; determining a current motion limit speed of the target instrument in the current period corresponding to a motor motion limit speed preset for a target motor according to the desired motion angle; determining a target motion speed of the target instrument in the current period according to a numerical relationship between the desired motion speed and the current motion limit speed, and controlling the target motor to move according to the target motion speed, so as to drive the motion of the target instrument by the motion of the target motor. The technical scheme of the embodiments of the present application can minimize the delay of the operation of the target instrument as far as possible on the premise of preventing the target motor from flying.

Description

TECHNICAL FIELD

[0001] The embodiment of the present application relates to the technical field of motion control, in particular to a motion control method, device and controller of a robot instrument. BACKGROUND

[0002] In recent years, with the development of electronic computers and industrial control technology, laparoscopic surgery robots have been rapidly developed and applied. As a product combining medicine and engineering, it has the characteristics of small trauma area, fast postoperative recovery and suitability for fine operation, and is chosen by more and more patients.

[0003] The robot mainly consists of a doctor console and a patient surgery platform, and the two parts exchange data through transmission lines. Therefore, the doctor can control the mechanical arm on the surgery platform by operating the operating hand on the console, that is, the movement of the surgical instrument connected to the mechanical arm, so as to perform surgical operation. For example, the surgical instrument is hung on the slide table of the mechanical arm and connected with the motor on the slide table. In this way, the motor can be controlled to move according to the movement of the operating hand, thereby driving the surgical instrument to move.

[0004] It should be noted that the motor has a rated power, and when the actual power of the motor is greater than the rated power, the motor may not be controlled, that is, the motor may appear to be out of control, which may affect the service life of the motor. At present, the movement speed of the surgical instrument is mainly limited to avoid the out-of-control phenomenon of the target motor.

[0005] However, the current movement speed limiting scheme for the surgical instrument, or the movement control scheme based on the speed limiting strategy, may cause the surgical instrument to be unable to quickly follow the movement of the operating hand, that is, there is a delay in the operation of the surgical instrument, which will affect the operation experience of the doctor and needs to be solved urgently. SUMMARY

[0006] The embodiment of the present application provides a motion control method, device and controller of a robot instrument to minimize the delay in the operation of the target instrument while preventing the target motor from being out of control.

[0007] According to an aspect of the present application, a motion control method of a robot instrument is provided. The method can be applied to a controller, the controller corresponds to a mechanical arm in a robot, and a target instrument connected to the mechanical arm moves under the driving of a target motor in the mechanical arm. The method comprises the following steps:

[0008] obtaining an expected motion angle of the target instrument in a current period and an actual motion angle of the target instrument in a previous period of the current period, and determining an expected motion speed of the target instrument in the current period according to the expected motion angle and the actual motion angle;

[0009] The current motion limit speed of the target instrument in the current period is determined according to the expected motion angle and the motor motion limit speed preset for the target motor.

[0010] The target motion speed of the target instrument in the current period is determined according to the numerical relationship between the expected motion speed and the current motion limit speed, and the target motor is controlled according to the target motion speed to drive the motion of the target instrument through the motion of the target motor.

[0011] According to another aspect of the present application, a motion control device of a robotic instrument is provided, which is configured on a controller corresponding to a mechanical arm in a robot, and a target instrument connected with the mechanical arm is driven to move by a target motor in the mechanical arm. The device comprises:

[0012] An expected motion speed determination module is configured to acquire an expected motion angle of the target instrument in the current period and an actual motion angle of the target instrument in a previous period of the current period, and determine an expected motion speed of the target instrument in the current period according to the expected motion angle and the actual motion angle.

[0013] A current motion limit speed determination module is configured to determine a current motion limit speed of the target instrument in the current period corresponding to the motor motion limit speed according to the expected motion angle, for the motor motion limit speed preset for the target motor.

[0014] A motion control module is configured to determine a target motion speed of the target instrument in the current period according to the numerical relationship between the expected motion speed and the current motion limit speed, and control the target motor to move according to the target motion speed to drive the motion of the target instrument through the motion of the target motor.

[0015] According to another aspect of the present application, a controller corresponding to a mechanical arm in a robot is provided, and a target instrument connected with the mechanical arm is driven to move by a target motor in the mechanical arm. The controller can comprise:

[0016] At least one processor; and

[0017] A memory in communication connection with the at least one processor; wherein,

[0018] The memory stores a computer program executable by the at least one processor, and the computer program is executed by the at least one processor to enable the at least one processor to implement the motion control method of the robotic instrument provided by any embodiment of the present application when executed.

[0019] The technical scheme of the embodiment of the present application is to obtain the expected motion angle of the target instrument in the current period and the actual motion angle of the target instrument in the previous period of the current period, and determine the expected motion speed of the target instrument in the current period according to the expected motion angle and the actual motion angle, so that the target instrument can follow the motion of the operating hand well and avoid operation delay of the target instrument. Furthermore, according to the expected motion angle, the current motion speed corresponding to the motor motion speed limit of the target instrument in the current period is determined, compared with the same instrument motion speed limit in the full motion range of the target instrument, the dynamic determination of the current motion speed can not only avoid the fly-off phenomenon of the target motor, but also make the target instrument follow the motion of the operating hand as much as possible. According to the numerical relationship between the expected motion speed and the current motion speed limit, the target motion speed of the target instrument in the current period is determined, so that the target motor motion can be controlled according to the target motion speed to drive the motion of the target instrument through the motion of the target motor, and the motion control of the target instrument is realized. The above technical scheme dynamically determines the current motion speed corresponding to the motor motion speed limit through the real-time expected motion angle, and then controls the motion of the target instrument according to the current motion speed, so that the motor motion speed can be ensured within the motor motion speed limit, that is, the fly-off phenomenon of the target motor is prevented, and the delay of the target instrument operation is reduced as much as possible.

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

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

[0022] Figure 1 is a flow chart of a robot instrument motion control method according to an embodiment of the present application;

[0023] Figure 2 is a connection diagram of a target instrument and a mechanical arm in a robot instrument motion control method according to an embodiment of the present application;

[0024] Figure 3 is a flow chart of another robot instrument motion control method according to an embodiment of the present application;

[0025] Figure 4ais a schematic diagram of the relationship between the swing joint angle and the swing motor angle in the motion control method of the robot instrument according to an embodiment of the present application;

[0026] Figure 4b is a schematic diagram of the relationship between the swing joint angle and the deceleration ratio in the motion control method of the robot instrument according to an embodiment of the present application;

[0027] Figure 4c is a schematic diagram of the relationship between the swing joint angle and the swing joint speed limit in the motion control method of the robot instrument according to an embodiment of the present application;

[0028] Figure 5 is a structural diagram of a motion control device of a robot instrument according to an embodiment of the present application;

[0029] Figure 6 is a structural diagram of a controller for implementing the motion control method of the robot instrument according to an embodiment of the present application. DETAILED DESCRIPTION

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

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

[0032] Figure 1is a flowchart of a motion control method of a robot instrument provided in an embodiment of the present application. The embodiment can be applicable to the case of dynamically limiting the speed of the motion process of a target instrument connected to a robot, and in particular, to the case of dynamically limiting the speed of the motion process of a surgical instrument connected to a surgical robot. The method can be executed by a motion control device of a robot instrument provided in an embodiment of the present application, which can be implemented in the form of software and / or hardware, and can be integrated on a controller corresponding to a mechanical arm in a robot, i.e., the controller can be used to control the mechanical arm, and in particular, the target instrument connected to the mechanical arm, and the controller can be used to control the target motor corresponding to the target instrument in the mechanical arm, so as to drive the target instrument to move by controlling the motion of the target motor.

[0033] Referring to Figure 1 The method of the embodiment of the present application specifically comprises the following steps:

[0034] S110. Obtain the expected motion angle of the target instrument in the current period and the actual motion angle of the target instrument in the last period of the current period, and determine the expected motion speed of the target instrument in the current period according to the expected motion angle and the actual motion angle.

[0035] The target instrument can be understood as an instrument to be controlled to move which is connected to the mechanical arm. In combination with the application scenarios that can be involved in the embodiment of the present application, the target instrument can be, for example, a surgical instrument, a rehabilitation instrument or an examination instrument, etc., which is related to the actual application scenario and is not limited here. It can be understood that, in the case that the target instrument is a surgical instrument, the robot to which the mechanical arm belongs is a surgical robot.

[0036] The full motion range of the target instrument can be divided into multiple periods, and the time interval T between each two adjacent periods is the same. The current period can be understood as the period in which the target instrument currently stays in the multiple periods, and correspondingly, the last period can be understood as the period in which the target instrument last stayed in the multiple periods.

[0037] The expected motion angle can be understood as the angle at which the target instrument is expected to move in the current period, and the angle can be determined according to the angle at which the operator hand in the robot actually moves in the current period. The actual motion angle can be understood as the angle at which the target instrument actually moves in the last period.

[0038] The expected motion angle and the actual motion angle are obtained. Further, according to the obtained two motion angles, the expected motion speed is determined, which can be understood as the speed of the target device when performing motion in the current period, so that the target device can move from the actual motion angle to the expected motion angle in the current period based on the expected motion speed. In actual application, optionally, the time interval between the current period and the previous period and the motion angle difference between the expected motion angle and the actual motion angle can be obtained, and then the expected motion speed is determined according to the time interval and the motion angle difference.

[0039] S120. According to the expected motion angle, the current motion limit speed of the target device in the current period corresponding to the motor motion limit speed preset for the target motor is determined.

[0040] The motor motion limit speed can be understood as the maximum speed that the target motor can reach in the motion process, which is preset to avoid the phenomenon of the target motor flying. In actual application, optionally, the motor motion limit speed can be obtained according to the rated motion speed of the target motor (i.e. the rotating speed of the target motor under rated power), for example, a preset proportion can be floated upwards on the basis of the rated motion speed to obtain the motor motion limit speed.

[0041] It can be understood that the target device moves under the driving of the target motor, and then the motor motion speed of the target motor directly affects the device motion speed of the target device, so that the device motion limit speed corresponding to the motor motion limit speed of the target device can be controlled within the device motion limit speed, so as to ensure that the motor motion speed is within the motor motion limit speed, and avoid the phenomenon of the target motor flying.

[0042] On this basis, it is found through practice that even if the motor motion limit speed is fixed, the device motion limit speed under different device motion angles may be different, which is particularly obvious in the swing joint of the target device. Since the expected motion angle is the device motion angle of the target device in the current period, the current motion limit speed can be determined according to the expected motion angle, which is the device motion limit speed applied by the target device in the current period. Here, by dynamically determining the current motion limit speed, not only can the phenomenon of the target motor flying be avoided, but also compared with using the same device motion limit speed in the full motion range of the target device, the target device can follow the motion of the operator as much as possible, and the delay of the target device operation can be reduced.

[0043] S130. According to the numerical relationship between the expected motion speed and the current motion limit speed, the target motion speed of the target device in the current period is determined, and the target motor is controlled to move according to the target motion speed, so as to drive the motion of the target device through the motion of the target motor.

[0044] wherein a numerical relationship between the expected motion speed and the current motion limit speed is obtained, the numerical relationship can represent that the expected motion speed is within the current motion limit speed, and the target motion speed of the target device in the current period can be determined according to the numerical relationship, the target motion speed can be understood as a speed finally adopted by the target device when moving in the current period. For example, in the case that the numerical relationship represents that the expected motion speed is within the current motion limit speed, the expected motion speed can be taken as the target motion speed; otherwise, the current motion limit speed can be taken as the target motion speed. The above example can minimize the delay of the target device operation as much as possible under the premise of avoiding the fly-off phenomenon of the target motor.

[0045] Further, the target motor is controlled according to the target motion speed to drive the movement of the target device by the movement of the target motor, specifically to drive the target device to move at the target motion speed, so as to realize the movement control of the target device. For example, the target motion angle of the target device in the current period can be determined according to the actual motion angle, the target motion speed and the time interval between the current period and the last period; further, the target motor is controlled to move in the current period according to the target motion angle, so as to drive the target device to move to the target motion angle in the current period, and realize the movement control of the target device.

[0046] The technical scheme of the embodiment of the present application obtains the expected motion angle of the target device in the current period and the actual motion angle in the last period of the current period, and determines the expected motion speed of the target device in the current period according to the expected motion angle and the actual motion angle, the expected motion speed can make the target device follow the movement of the operator well and avoid the delay of the target device operation; further, the current motion limit speed corresponding to the motor motion limit speed of the target device in the current period is determined according to the expected motion angle, compared with using the same device motion limit speed in the full motion range of the target device, the dynamic determination of the current motion limit speed can not only avoid the fly-off phenomenon of the target motor, but also make the target device follow the movement of the operator as much as possible; the target motion speed of the target device in the current period is determined according to the numerical relationship between the expected motion speed and the current motion limit speed, so that the movement of the target motor can be controlled according to the target motion speed to drive the movement of the target device by the movement of the target motor, and the movement control of the target device is realized. The above technical scheme dynamically determines the current motion limit speed corresponding to the motor motion limit speed according to the real-time expected motion angle, and then controls the movement of the target device according to the current motion limit speed, so as to not only ensure that the motor motion speed is within the motor motion limit speed, i.e. prevent the fly-off phenomenon of the target motor, but also minimize the delay of the target device operation as much as possible.

[0047] In combination with the application scenarios that the embodiments of the present application can be involved in, see Figure 2 The target instrument 1 generally has four motor wheels corresponding to the swing joint, the pitch joint, the left yaw joint and the right yaw joint on the target instrument 1 respectively, and the four motor wheels are connected after being matched with the four motors on the slide table of the mechanical arm 2 (i.e. the motors 3-6 in the figure) respectively, so that the four joints can be driven to move by controlling the movement of the four motors, and the effective work of the target instrument 1 in a certain space can be realized.

[0048] In combination with the instrument structure design of the target instrument that can be applied in the embodiments of the present application, it can be known that the transmission ratio between the instrument movement angle of the swing joint and the motor movement angle of the swing motor (i.e. the motor corresponding to the swing joint) is fixed and unchangeable, so that the instrument movement speed limit corresponding to the motor movement speed limit of the swing joint is fixed and unchangeable; the same is true for the pitch joint. However, the transmission ratio between the instrument movement angle of the yaw joint (i.e. the left yaw joint and the right yaw joint) and the motor movement angle of the yaw motor (i.e. the motor corresponding to the yaw joint) is not fixed and unchangeable, so that the instrument movement speed limit corresponding to the motor movement speed limit of the yaw joint is not fixed and unchangeable, and therefore the instrument movement speed limit of the yaw joint can be dynamically determined, which is as follows:

[0049] On the basis of any of the above technical solutions, optionally, the target motor is a yaw motor corresponding to the yaw joint in the target instrument, and the expected movement angle, the actual movement angle, the expected movement speed, the current movement speed limit and the target movement speed all correspond to the yaw joint, the target motor is controlled according to the target movement speed to drive the movement of the target instrument through the movement of the target motor, including:

[0050] The yaw motor is controlled according to the target movement speed to drive the movement of the yaw joint through the movement of the yaw motor, so that the dynamic speed limit control of the yaw joint is realized.

[0051] Figure 3 is a flowchart of another robot instrument movement control method provided in the embodiments of the present application. The present embodiment is optimized on the basis of the above technical solutions. In the present embodiment, optionally, the current movement speed limit corresponding to the motor movement speed limit of the target instrument in the current period is determined according to the expected movement angle, including: obtaining a first relationship pre-constructed based on the motor movement speed limit, wherein the first relationship represents the relationship between the instrument movement angle of the target instrument and the instrument movement speed limit of the target instrument, and the expected movement angle is the expected instrument movement angle of the target instrument in the current period; and determining the current movement speed limit corresponding to the motor movement speed limit of the target instrument in the current period according to the first relationship and the expected movement angle, wherein the current movement speed limit is the instrument movement speed limit applied by the target instrument in the current period. The explanations of the same or corresponding terms as in the above embodiments are not repeated here.

[0052] Referring to Figure 3 The method of the embodiment can specifically include the following steps:

[0053] S210. Obtain a desired motion angle of the target instrument in a current period and an actual motion angle of the target instrument in a previous period of the current period, and determine a desired motion speed of the target instrument in the current period according to the desired motion angle and the actual motion angle.

[0054] S220. Obtain a first relationship pre-constructed based on a motor motion speed limit, wherein the motor motion speed limit is preset for a target motor, the first relationship represents a relationship between an instrument motion angle of the target instrument and an instrument motion speed limit of the target instrument, and the desired motion angle is a desired instrument motion angle of the target instrument in the current period.

[0055] The first relationship is pre-constructed according to the motor motion speed limit, and is used to represent the relationship between the instrument motion angle and the instrument motion speed limit. The relationship can be represented by an equation, an expression, a diagram, or a table, which is related to the actual situation and is not specifically limited herein. The first relationship is obtained.

[0056] S230. Determine a current motion speed limit corresponding to the motor motion speed limit of the target instrument in the current period according to the first relationship and the desired motion angle, wherein the current motion speed limit is an instrument motion speed limit applied by the target instrument in the current period.

[0057] The first relationship represents the relationship between the instrument motion angle and the instrument motion speed limit, and the desired motion angle is a desired instrument motion angle of the target instrument in the current period. Therefore, the current motion speed limit corresponding to the motor motion speed limit of the target instrument in the current period can be determined according to the first relationship and the desired motion angle.

[0058] S240. Determine a target motion speed of the target instrument in the current period according to a numerical relationship between the desired motion speed and the current motion speed limit, and control the target motor to move according to the target motion speed, so as to drive the movement of the target instrument through the movement of the target motor.

[0059] The technical scheme of the embodiment of the application pre-constructs the first relationship representing the relationship between the instrument motion angle and the instrument motion speed limit, so that the first relationship and the desired motion angle can be used to quickly and accurately determine the current motion speed limit.

[0060] An optional technical scheme, the first relationship is pre-constructed by the following steps:

[0061] obtaining a second relationship pre-constructed, wherein the second relationship represents a relationship between the instrument motion angle and the target instrument deceleration ratio;

[0062] constructing the first relationship according to the motor motion speed limit and the second relationship.

[0063] The deceleration ratio is an example of the transmission ratio, and the target instrument deceleration ratio can be understood as a ratio of the motor motion angle to the instrument motion angle, and can be further understood as a ratio of the motor motion speed to the instrument motion speed, i.e., can be further understood as a ratio of the motor motion speed limit to the instrument motion speed limit.

[0064] The second relationship represents a relationship between the instrument motion angle and the deceleration ratio, and therefore, the first relationship can be constructed according to the motor motion speed limit and the second relationship in combination with the meaning of the deceleration ratio.

[0065] The above technical solution uses the second relationship to accurately construct the first relationship.

[0066] On this basis, optionally, the second relationship is pre-constructed by the following steps:

[0067] obtaining a third relationship pre-constructed, wherein the third relationship represents a relationship between the instrument motion angle and the target motor motor motion angle;

[0068] constructing the second relationship according to the third relationship, wherein the deceleration ratio is a ratio of the motor motion angle to the instrument motion angle.

[0069] Since the third relationship represents a relationship between the instrument motion angle and the motor motion angle, and the deceleration ratio is a ratio of the motor motion angle to the instrument motion angle, the second relationship representing a relationship between the instrument motion angle and the deceleration ratio can be constructed according to the third relationship, thereby accurately constructing the second relationship.

[0070] On this basis, optionally, the third relationship is represented by a motion angle calculation equation, one side of the motion angle calculation equation is the motor motion angle and the other side is a first angle calculation formula based on the instrument motion angle; the first relationship is represented by a motion speed limit calculation equation, one side of the motion speed limit calculation equation is the instrument motion speed limit and the other side is a product of the motor motion speed limit and a second angle calculation formula, and the second angle calculation formula is a ratio of the instrument motion angle to the first angle calculation formula.

[0071] The first angle calculation formula can be understood as a formula for calculating the motor movement angle based on the instrument movement angle, so the motor movement angle can be taken as one side and the first angle calculation formula can be taken as the other side to obtain a movement angle calculation equation for calculating the motor movement angle. On this basis, further, the second angle calculation formula can be expressed as the ratio of the instrument movement angle to the first angle calculation formula, that is, the inverse of the deceleration ratio, so the instrument movement speed limit can be taken as one side and the product of the motor movement speed limit and the second angle calculation formula can be taken as the other side to obtain a movement speed limit calculation equation for calculating the instrument movement speed limit.

[0072] The above technical solution, by equation expression relationship, is beneficial to the fast and accurate calculation of the instrument movement speed limit.

[0073] In order to better understand the above-mentioned various technical solutions as a whole, the following will be exemplarily described in combination with specific examples. Exemplarily, here the surgical instrument connected with the laparoscopic surgery robot is taken as an example, and the movement control process of the yaw joint in the surgical instrument is as follows:

[0074] The physical model of the yaw joint (i.e., the movement angle calculation equation above) is as follows:

[0075] Motor = A * (asin(2 * sin(Ins + B)) - C);

[0076] wherein Motor is the motor movement angle of the yaw motor (hereinafter can be referred to as the yaw motor angle); Ins is the instrument movement angle of the yaw joint (hereinafter can be referred to as the yaw joint angle); A, B and C are all inherent parameters of the surgical instrument, which are constant values related to the instrument structure of the surgical instrument; A * (asin(2 * sin(Ins + B)) - C) is the first angle calculation formula.

[0077] According to the above movement angle calculation equation, the relationship between Ins and Motor can be obtained as shown in Figure 4a On this basis, the deceleration ratio of the yaw joint is defined as Motor / Ins, and the results of the deceleration ratio under different Ins can be obtained as shown in Figure 4b Based on the above characteristics of the yaw joint, the dynamic speed limit process is designed as follows:

[0078] Step one: obtain the performance of the yaw motor. The rated speed of the yaw motor is denoted as ratedMotorSpeed, which can be floated by 10% upward in actual use, that is, ratedMotorSpeed*110% is taken as the motor movement speed limit (hereinafter can be referred to as the yaw motor speed limit) of the yaw motor, which is taken as the maximum value of the motor movement speed (hereinafter can be referred to as the yaw motor speed) of the yaw motor.

[0079] Step two: According to the above description, under different Ins, the deceleration ratio is different, and the two are nonlinear. In use, the yaw motor controls the yaw joint movement, so the instrument movement limit speed of the yaw joint (hereinafter referred to as the yaw joint limit speed) can be calculated by the following movement limit speed calculation equation:

[0080]

[0081] Wherein, InsLimitSpeed is the yaw joint limit speed, the unit is degree / millisecond; is the second angle calculation formula. On this basis, the corresponding curve is shown in Figure 4c .

[0082] Step three: According to the movement limit speed calculation equation obtained in step two, the following dynamic limit speed process is designed:

[0083] (1) Based on the sampling period T (that is, the time interval in the above), the expected movement angle a of the yaw joint is obtained, a is the real-time position information of the yaw joint, as input, denoted as lrIn=a.

[0084] (2) Record lrIn and the output value lrOut_old (that is, the actual movement angle in the above) of the yaw joint at the last cycle, and calculate the movement angle difference value between the two in the current cycle, and convert it into expected movement speed lrDelta, wherein lrDelta=(lrIn-lrOut_old) / T.

[0085] (3) Judge the numerical relationship between lrDelta and InsLimitSpeed, and assign the intermediate variable lrTemp (that is, the target movement speed in the above) based on the numerical relationship. Specifically:

[0086]

[0087] (4) According to lrTemp, the output value lrOut (that is, the target movement angle in the above) of the current cycle after speed limiting can be obtained as:

[0088] lrOut=lrOut_old+lrTemp*T.

[0089] Send lrOut to the driver to control the yaw motor movement, so as to ensure that the yaw motor does not over-speed in the full movement range, and can maximize the followability of the yaw joint.

[0090] Figure 5A structural block diagram of a motion control device of a robot instrument is provided in the embodiments of the present application, and the device is used to execute the motion control method of the robot instrument provided in any of the above embodiments. The device and the motion control method of the robot instrument in each of the above embodiments belong to the same inventive concept, and the details not described in the embodiments of the motion control device of the robot instrument can be referred to the embodiments of the motion control method of the robot instrument. Referring to Figure 5 The device is configured on a controller, the controller corresponds to a mechanical arm in a robot, and a target instrument connected to the mechanical arm is driven to move by a target motor in the mechanical arm. The device includes an expected motion speed determination module 310, a current motion speed limit determination module 320, and a motion control module 330.

[0091] The expected motion speed determination module 310 is configured to obtain an expected motion angle of the target instrument in a current period and an actual motion angle of the target instrument in a previous period of the current period, and determine an expected motion speed of the target instrument in the current period according to the expected motion angle and the actual motion angle.

[0092] The current motion speed limit determination module 320 is configured to determine a current motion speed limit of the target instrument in the current period corresponding to the motor motion speed limit pre-set for the target motor according to the expected motion angle.

[0093] The motion control module 330 is configured to determine a target motion speed of the target instrument in the current period according to a numerical relationship between the expected motion speed and the current motion speed limit, and control the target motor to move according to the target motion speed, so as to drive the target instrument to move through the movement of the target motor.

[0094] Optionally, the current motion speed limit determination module 320 can include:

[0095] A first relationship obtaining unit is configured to obtain a first relationship pre-constructed based on the motor motion speed limit, wherein the first relationship represents a relationship between an instrument motion angle of the target instrument and an instrument motion speed limit of the target instrument, and the expected motion angle is an expected instrument motion angle of the target instrument in the current period.

[0096] A current motion speed limit determination unit is configured to determine a current motion speed limit of the target instrument in the current period corresponding to the motor motion speed limit according to the first relationship and the expected motion angle, wherein the current motion speed limit is an instrument motion speed limit applied by the target instrument in the current period.

[0097] On this basis, optionally, the first relationship is pre-constructed by the following modules:

[0098] a second relationship acquisition module, configured to acquire a second relationship that is pre-constructed, wherein the second relationship represents a relationship between an instrument movement angle and a deceleration ratio of a target instrument;

[0099] a first relationship construction module, configured to construct a first relationship according to the motor movement limit speed and the second relationship.

[0100] On this basis, optionally, the second relationship is pre-constructed by the following modules:

[0101] a third relationship acquisition module, configured to acquire a third relationship that is pre-constructed, wherein the third relationship represents a relationship between the instrument movement angle and a motor movement angle of a target motor;

[0102] a second relationship construction module, configured to construct the second relationship according to the third relationship, wherein the deceleration ratio is a ratio of the motor movement angle to the instrument movement angle.

[0103] On this basis, optionally, the third relationship is represented by a movement angle calculation equation, one side of the movement angle calculation equation is the motor movement angle and the other side is a first angle calculation formula based on the instrument movement angle; correspondingly, the first relationship is represented by a movement limit speed calculation equation, one side of the movement limit speed calculation equation is the instrument movement limit speed and the other side is a product of the motor movement limit speed and a second angle calculation formula, the second angle calculation formula is a ratio of the instrument movement angle to the first angle calculation formula.

[0104] Optionally, the movement control module 330 can include:

[0105] a numerical relationship obtaining unit, configured to obtain a numerical relationship between a desired movement speed and a current movement limit speed;

[0106] a target movement speed first determination unit, configured to, in a case where the numerical relationship represents that the desired movement speed is within the current movement limit speed, determine the desired movement speed as a target movement speed of the target instrument in a current period;

[0107] a target movement speed second determination unit, configured to, otherwise, determine the current movement limit speed as the target movement speed.

[0108] Optionally, the movement control module 330 can include:

[0109] a target movement angle determination unit, configured to determine a target movement angle of the target instrument in the current period according to an actual movement angle, the target movement speed, and a time interval between the current period and a previous period;

[0110] a first movement control unit, configured to control the target motor to move in the current period according to the target movement angle.

[0111] On the basis of any of the above devices, optionally, the target motor is a yaw motor corresponding to a yaw joint in the target instrument, the expected motion angle, the actual motion angle, the expected motion speed, the current motion speed limit, and the target motion speed all correspond to the yaw joint, and the motion control module 330 can include:

[0112] A second motion control unit is configured to control the yaw motor to move according to the target motion speed, so as to drive the yaw joint to move through the movement of the yaw motor.

[0113] The motion control device of the robotic instrument provided in the embodiments of the present application can obtain the expected motion angle of the target instrument in the current period and the actual motion angle of the target instrument in the previous period of the current period through the expected motion speed determination module, and determine the expected motion speed of the target instrument in the current period according to the expected motion angle and the actual motion angle. The expected motion speed can make the target instrument follow the movement of the operator well and avoid operation delay of the target instrument. Further, the current motion speed limit corresponding to the motor motion speed limit of the target instrument in the current period is determined according to the expected motion angle through the current motion speed limit determination module. Compared with using the same instrument motion speed limit in the full motion range of the target instrument, the dynamic determination of the current motion speed limit can not only avoid the runaway phenomenon of the target motor, but also make the target instrument follow the movement of the operator as much as possible. The target motion speed of the target instrument in the current period is determined according to the numerical relationship between the expected motion speed and the current motion speed limit through the motion control module, so that the movement of the target motor can be controlled according to the target motion speed, the movement of the target instrument is driven through the movement of the target motor, and the motion control of the target instrument is realized. Through the above device, the current motion speed limit corresponding to the motor motion speed limit is dynamically determined according to the real-time expected motion angle, and then the motion control of the target instrument is performed according to the current motion speed limit. Therefore, the motor motion speed can be ensured to be within the motor motion speed limit, i.e., the runaway phenomenon of the target motor is prevented, and the delay of the operation of the target instrument is reduced as much as possible.

[0114] The motion control device of the robotic instrument provided in the embodiments of the present application can execute the motion control method of the robotic instrument provided in any of the embodiments of the present application, and has the corresponding function modules and beneficial effects of the execution method.

[0115] It should be noted that in the embodiments of the above motion control device of the robotic instrument, each unit and module included is only divided according to the function logic, but is not limited to the above division, as long as the corresponding function can be realized; in addition, the specific names of each functional unit are only for convenient mutual differentiation, and do not limit the protection scope of the present application.

[0116] Figure 6A schematic diagram of a controller 10, which can be used to implement embodiments of the present invention, is shown. The controller 10 corresponds to a robotic arm in a robot, and a target device connected to the robotic arm moves under the drive of a target motor within the robotic arm. The controller is intended to represent various forms of digital computers, such as laptop computers, desktop computers, workbenches, personal digital assistants, servers, blade servers, mainframe computers, and other suitable computers. The controller can also represent various forms of mobile devices, such as personal digital processors, cellular phones, smartphones, wearable devices (such as helmets, glasses, watches, etc.), and other similar computing devices. The components shown herein, their connections and relationships, and their functions are merely illustrative and are not intended to limit the implementation of the invention described and / or claimed herein.

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

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

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

[0120] In some embodiments, the motion control method of the robotic instrument can be implemented as a computer program tangibly embodied in a computer readable storage medium, e.g., storage unit 18. In some embodiments, portions of or all of the computer program can be loaded onto the controller 10 via the ROM 12 and / or the communications unit 19. When a computer program is loaded onto the RAM 13 and executed by the processor 11, one or more steps of the motion control method of the robotic instrument as described above can be performed. Alternatively, in other embodiments, the processor 11 can be configured to perform the motion control method of the robotic instrument by other means, e.g., with the aid of firmware.

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

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

[0123] In the context of the present application, a computer-readable storage medium can be a tangible medium that can contain or store a computer program for use by or in connection with an instruction execution system, apparatus, or device. A computer-readable storage medium can include, but is not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any suitable combination of the foregoing. Alternatively, a computer-readable storage medium can be a machine-readable signal medium. More specific examples of a machine-readable storage medium will include one or more lines of a program of instructions in a transitory signal, a portable computer diskette, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or Flash memory), an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.

[0124] To provide for interaction with a user, the systems and techniques described here can be implemented on a controller that has a display (e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor) for displaying information to the user and a keyboard and a pointing device (e.g., a mouse or a trackball) by which the user can provide input to the controller. Other kinds of devices can be used to provide for interaction with a user as well; for example, feedback provided to the user can be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and input from the user can be received in any form, including acoustic, speech, or tactile input.

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

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

[0127] It should be understood that the various forms of flow shown above can be used to reorder, add or delete steps. For example, each step described in the present application can be executed in parallel, sequentially or in a different order, as long as the desired results of the technical solutions of the present application can be achieved, which is not limited herein.

[0128] The above detailed description does not constitute a limitation on the scope of protection of the present application. Those skilled in the art should understand that various modifications, combinations, sub-combinations and substitutions can be made according to design requirements and other factors. Any modifications, equivalent replacements and improvements made within the spirit and principles of the present application shall be included in the scope of protection of the present application.

Claims

1. A motion control device for a robotic instrument, characterized in that, The device is configured on a controller corresponding to a mechanical arm in a robot, and a target instrument connected to the mechanical arm moves under the driving of a target motor in the mechanical arm. The device comprises: an expected motion speed determination module configured to acquire an expected motion angle of the target instrument in a current period and an actual motion angle of the target instrument in a previous period of the current period, and determine an expected motion speed of the target instrument in the current period according to the expected motion angle and the actual motion angle; a current motion speed limit determination module configured to determine, according to the expected motion angle, a current motion speed limit of the target instrument in the current period corresponding to a motor motion speed limit preset for the target motor; 2. The apparatus of claim 1, wherein, a motion control module configured to determine a target motion speed of the target instrument in the current period according to a numerical relationship between the expected motion speed and the current motion speed limit, and control the target motor to move according to the target motion speed, so as to drive the motion of the target instrument through the motion of the target motor. The current motion speed limit determination module comprises: a first relationship acquisition unit configured to acquire a first relationship pre-constructed based on the motor motion speed limit, wherein the first relationship represents a relationship between an instrument motion angle of the target instrument and an instrument motion speed limit of the target instrument, and the expected motion angle is the instrument motion angle expected by the target instrument in the current period; 3. The apparatus of claim 2, wherein, a current motion speed limit determination unit configured to determine, according to the first relationship and the expected motion angle, a current motion speed limit of the target instrument in the current period corresponding to the motor motion speed limit, wherein the current motion speed limit is the instrument motion speed limit applied by the target instrument in the current period. The first relationship is pre-constructed by the following modules: a second relationship acquisition module configured to acquire a second relationship pre-constructed, wherein the second relationship represents a relationship between the instrument motion angle and a deceleration ratio of the target instrument; 4. The apparatus of claim 3, wherein, a first relationship construction module configured to construct the first relationship according to the motor motion speed limit and the second relationship. The second relationship is pre-constructed by the following modules: a third relationship acquisition module configured to acquire a third relationship pre-constructed, wherein the third relationship represents a relationship between the instrument motion angle and a motor motion angle of the target motor; 5. The apparatus of claim 4, wherein, a second relationship construction module configured to construct the second relationship according to the third relationship, wherein the deceleration ratio is a ratio of the motor motion angle to the instrument motion angle. The third relationship is represented by a motion angle calculation equation, one side of the motion angle calculation equation is the motor motion angle and the other side is a first angle calculation formula composed of the instrument motion angle. The first relationship is represented by a motion limit calculation equation, one side of which is the machine motion limit and the other side is the product of the motor motion limit and a second angle calculation equation, the second angle calculation equation being the ratio of the machine motion angle and the first angle calculation equation.

6. The apparatus of claim 1, wherein, The motion control module comprises: a numerical relationship obtaining unit configured to obtain a numerical relationship between the desired motion speed and the current motion limit; a target motion speed first determining unit configured to, in a case where the numerical relationship represents that the desired motion speed is within the current motion limit, determine the desired motion speed as the target motion speed of the target machine in the current period; a target motion speed second determining unit configured to, otherwise, determine the current motion limit as the target motion speed.

7. The apparatus of claim 1, wherein, The motion control module comprises: a target motion angle determining unit configured to determine a target motion angle of the target machine in the current period according to the actual motion angle, the target motion speed, and a time interval between the current period and the previous period; a first motion control unit configured to control the target motor to move in the current period according to the target motion angle.

8. The apparatus of any one of claims 1-7, wherein, The target motor is a swing motor corresponding to a swing joint in the target machine, and the desired motion angle, the actual motion angle, the desired motion speed, the current motion limit, and the target motion speed all correspond to the swing joint, and the motion control module comprises: a second motion control unit configured to control the swing motor to move according to the target motion speed, so as to drive the movement of the swing joint through the movement of the swing motor.

9. A controller characterized by comprising: Corresponding to a mechanical arm in a robot, a target machine connected to the mechanical arm moves under the driving of a target motor in the mechanical arm, and the controller comprises: at least one processor; and a memory connected to the at least one processor in communication; wherein the memory stores a computer program executable by the at least one processor, and the computer program is executed by the at least one processor to cause the at least one processor to perform the steps represented by the motion control device of the robot machine as claimed in any one of claims 1-8.

Images