Trajectory control method and device

Abstract

The application provides a trajectory control method and device. The method comprises the following steps: obtaining a desired position of a master hand mechanical arm joint and a current position of the master hand mechanical arm joint; determining speed-time planning data of the master hand mechanical arm joint according to the desired position of the master hand mechanical arm joint and the current position of the master hand mechanical arm joint; determining an output torque of the master hand mechanical arm joint within a planning time length according to the speed-time planning data; and driving a motor of the master hand mechanical arm joint according to the output torque. The method is used to improve the control precision of the master hand.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of medical devices, and in particular to a trajectory control method and device. BACKGROUND

[0002] When a slave hand of a surgical robot is switched or switched out, the master hand position needs to be re-determined according to the position of the slave hand to avoid the slave hand from hurting the patient. When the master hand position is re-determined, the master hand needs to be tracked.

[0003] In related technologies, when the master hand trajectory is tracked, the control system plans the motion trajectory of the master hand robot arm joint according to the task requirements, including position, speed, acceleration and other parameters, calculates the required joint torque or force according to the planned trajectory using the inverse dynamics model, and adjusts the deviation between the actual motion and the expected trajectory using a preset proportional-derivative (PD) controller. Then, the control system combines the feedforward compensation signal calculated by the inverse dynamics model with the feedback control signal output by the PD controller to form the final control command sent to the robot actuator. The robot executes the control command and feeds back the actual motion state through the sensor for the next control cycle.

[0004] However, the above trajectory control method requires high computing power of the control system, resulting in low speed control accuracy of the master hand. SUMMARY

[0005] The present application provides a trajectory control method and device to solve the problem of low control accuracy of the master hand in the prior art trajectory control method.

[0006] In a first aspect, the present application provides a trajectory control method, comprising:

[0007] obtaining an expected position of a master hand robot arm joint and a current position of the master hand robot arm joint;

[0008] determining speed-time planning data of the master hand robot arm joint according to the expected position of the master hand robot arm joint and the current position of the master hand robot arm joint;

[0009] determining an output torque of the master hand robot arm joint within a planning time period according to the speed-time planning data;

[0010] driving a motor of the master hand robot arm joint according to the output torque.

[0011] In some embodiments, determining the speed-time planning data of the master hand robot arm joint according to the expected position of the master hand robot arm joint and the current position of the master hand robot arm joint comprises:

[0012] determine a total distance of the master manipulator joint according to the desired position of the master manipulator joint and a current position of the master manipulator joint;

[0013] determine a time length of each speed stage according to the preset speed planning algorithm and the total distance of the master manipulator joint, wherein each speed stage comprises an acceleration stage and a deceleration stage;

[0014] determine speed-time planning data of the master manipulator joint according to the total distance of the master manipulator joint and the time length of each speed stage.

[0015] In some embodiments, determining the time length of each speed stage according to the preset speed planning algorithm and the total distance of the master manipulator joint comprises:

[0016] determine a time proportion of each speed stage according to the preset speed planning algorithm, a speed threshold and an acceleration threshold;

[0017] determine the time length of each speed stage according to the time proportion of each speed stage and the total distance of the master manipulator joint.

[0018] In some embodiments, driving the motor of the master manipulator joint according to the output torque comprises:

[0019] filter the output torque by using a low-pass filter;

[0020] drive the motor of the master manipulator joint according to the filtered output torque.

[0021] In some embodiments, the process of driving the motor of the master manipulator joint according to the output torque comprises:

[0022] monitor a vibration frequency of the master manipulator joint;

[0023] when the vibration frequency is greater than a frequency threshold, continue to drive the motor of the master manipulator joint according to a preset torque.

[0024] In some embodiments, obtaining the desired position of the master manipulator joint comprises:

[0025] obtain a current position of a slave manipulator joint;

[0026] determine the desired position of the master manipulator joint by inverse solution calculation according to the current position of the slave manipulator joint.

[0027] In some embodiments, obtaining the current position of the slave manipulator joint comprises:

[0028] The current position of the slave robot arm currently controlled by the master robot arm is acquired, the slave robot arm currently controlled by the master robot arm being determined according to the switching instruction, the switching instruction being used to instruct switching of the slave robot arm currently controlled by the master robot arm.

[0029] In some embodiments, the output torque of the master robot arm joint within the planning duration is determined according to the speed-time planning data, comprising:

[0030] The output torque of the master robot arm joint at the next moment is determined according to the actual position and the actual speed of the master robot arm joint at the last moment, and the expected position and the expected speed of the master robot arm joint at the next moment.

[0031] In a second aspect, the present application provides a trajectory control device, comprising:

[0032] The acquisition module is used to acquire the expected position of the master robot arm joint and the current position of the master robot arm joint.

[0033] The determination module is used to determine the speed-time planning data of the master robot arm joint according to the expected position of the master robot arm joint and the current position of the master robot arm joint.

[0034] The determination module is further used to determine the output torque of the master robot arm joint within the planning duration according to the speed-time planning data.

[0035] The driving module is used to drive the motor of the master robot arm joint according to the output torque.

[0036] In a third aspect, the present application provides an electronic device, comprising a memory and a processor.

[0037] The memory is used to store a computer program, and the processor is used to execute the computer program stored by the memory to realize the trajectory control method in the first aspect and any one of the embodiments of the first aspect.

[0038] In a fourth aspect, the present application provides a computer readable storage medium, and the computer readable storage medium stores a computer program, the computer program being executed by a processor to realize the trajectory control method in the first aspect and any one of the embodiments of the first aspect.

[0039] In a fifth aspect, the present application provides a computer program product, and the computer program product comprises a computer program, the computer program being executed by a processor to realize the trajectory control method in the first aspect and any one of the embodiments of the first aspect.

[0040] The trajectory control method and device provided in the application determine the speed-time planning data of the master robot arm joint according to the expected position and the current position of the master robot arm joint, and then determine the output torque of the master robot arm joint within the planning duration, so as to drive the motor of the master robot arm joint, reduce the calculation power pressure of the master robot arm control system, and improve the speed control accuracy of the master robot arm. BRIEF DESCRIPTION OF DRAWINGS

[0041] In order to more clearly illustrate the technical solutions in the application or the prior art, the drawings needed to be used in the embodiments or the prior art description will be briefly introduced. Obviously, the drawings in the following description are some embodiments of the application, and other drawings can be obtained by those skilled in the art without creative labor.

[0042] Figure 1 A scene schematic diagram of trajectory control provided by an embodiment of the application;

[0043] Figure 2 A flowchart of a trajectory control method provided by an embodiment of the application;

[0044] Figure 3 A structure schematic diagram of a trajectory control device provided by an embodiment of the application;

[0045] Figure 4 A hardware structure schematic diagram of an electronic device provided by an embodiment of the application. DETAILED DESCRIPTION

[0046] In order to make the purpose, technical solutions and advantages of the application more clear, the technical solutions in the application will be clearly and completely described below in combination with the drawings in the application. Obviously, the described embodiments are some embodiments of the application, not all embodiments. Based on the embodiments in the application, all other embodiments obtained by those skilled in the art without creative labor are within the protection scope of the application.

[0047] At present, robots usually adopt PD control algorithm and proportional-integral-derivative (PID) control algorithm, for example, point control usually adopts PD control algorithm, and the control effect is also good. Compared with point control, trajectory control puts forward higher requirements on the control algorithm, which needs to closely observe the changes of robot speed and acceleration, otherwise it may cause damage to the robot due to excessive speed or acceleration, and even cause serious personal safety accidents.

[0048] In the related art, there are mainly the following trajectory control methods. One is to combine the inverse dynamics model and the PD controller, specifically, the control system plans the motion trajectory of the robot according to the task requirements, including position, velocity, acceleration and other parameters, calculates the required joint torque or force according to the planned trajectory by using the inverse dynamics model, and adjusts the deviation between the actual motion and the expected trajectory by using the preset PD controller. Then, the control system combines the feedforward compensation signal calculated by the inverse dynamics model with the feedback control signal output by the PD controller to form the final control command and sends it to the robot actuator. The robot executes the control command and feeds back the actual motion state through the sensor for the next control cycle. Two is to combine the inverse dynamics model and the variable PD controller, on the basis of the first method, dynamically adjust the parameters of the PD controller according to the actual situation to improve the control performance. Three is to combine feedback linearization, PD controller and feedforward compensation, specifically, the control system converts the nonlinear dynamic model of the robot into a linear model through state transformation or nonlinear control strategy, simplifies the control design, and designs a PD controller on the linearized system to process the deviation of the linearized system. The inverse dynamics model is used for feedforward compensation to further improve the tracking accuracy. The output of the PD controller is combined with the feedforward compensation signal to form the control command and execute it, while collecting feedback for subsequent control. Four is to combine feedback linearization, PID controller and feedforward compensation, on the basis of the third method, an integral term (I) is introduced to form a PID controller to eliminate the steady-state error of the system and improve the control accuracy.

[0049] The above trajectory control methods all use the inverse dynamics model as feedforward, which requires accurate robot system parameters to establish the inverse dynamics model. However, for complex robot systems, it is difficult to obtain accurate parameters, which increases the time cost and control difficulty, especially for robots with complex systems and weak computing power. The above methods have obvious shortcomings.

[0050] To solve the above problems, the present application provides a trajectory control method and device. In this method, the velocity-time planning of the master robot arm joint is performed according to the expected position of the master robot arm joint and the current position of the master robot arm joint, and the master robot arm joint is controlled to move to the expected position according to the planning result. In this method, the inverse dynamics model is not needed to be established, which saves time cost and is simple to calculate, and the requirement for the computing power of the control system is low, which improves the control accuracy of the velocity and acceleration of the master robot arm joint.

[0051] The technical solutions of the present application will be described in detail below with specific embodiments. The following specific embodiments can be combined with each other, and the same or similar concepts or processes may not be described in detail in some embodiments.

[0052] Figure 1 A scene diagram of a trajectory control provided by an embodiment of the present application is shown. As shown in the figure, Figure 1 The surgical robot includes a movable console and a slave robot arm, the end of the robot arm is usually connected with a multi-joint execution mechanism and a puncture device, the execution mechanism is used to connect a surgical instrument, and the puncture device can provide a channel for the surgical instrument, so that a single-hole minimally invasive surgery can be performed. When the surgical instrument needs to be replaced or the stroke of the surgical instrument is out of limit, the position of the slave robot arm needs to be controlled to remain unchanged, and the position of the master robot arm of the console is moved to make the master robot arm match the position of the slave robot arm again, so as to avoid hurting the patient on the slave side.

[0053] In the present application, an electronic device is taken as an execution subject to execute the trajectory control method of the following embodiments. Specifically, the execution subject can be a hardware device of the electronic device, or a software application implemented in the electronic device, or a computer readable storage medium installed with the software application implementing the following embodiments, or a code of the software application implementing the following embodiments.

[0054] Figure 2 A flowchart of a trajectory control method provided by an embodiment of the present application is shown. As shown in the figure, Figure 2 The method of the present embodiment can include the following steps:

[0055] S101, obtaining a desired position of a joint of a master robot arm and a current position of the joint of the master robot arm.

[0056] In the present embodiment, the electronic device can establish a stable communication connection with the master robot arm through a bus or a wireless manner, to ensure that the data can be obtained and sent in real time.

[0057] The master robot arm includes a plurality of joints, and for each joint, its current position and desired position need to be obtained. The electronic device can obtain the current position of each joint through a position sensor of the master robot arm, and obtain the current position of the slave robot arm through a position sensor of the slave robot arm, and determine the desired position of each joint of the master robot arm according to the position of the slave robot arm.

[0058] S102, determining speed-time planning data of the joint of the master robot arm according to the desired position of the joint of the master robot arm and the current position of the joint of the master robot arm.

[0059] In the present embodiment, the electronic device can determine the distance or angle to be moved of the joint according to the desired position and the current position of the joint of the master robot arm, and generate the speed-time planning data of each joint using an algorithm according to the distance or angle to be moved, the dynamics characteristics of the robot arm, the path smoothness requirement, etc., such as S-shaped curve planning, polynomial interpolation, etc.

[0060] The planning result includes the speed and acceleration of each joint at each time point, for example, the speed of joint A is v1 and the acceleration is a1 in the time period from t1 to t2, the speed is v2 and the acceleration is a2 in the time period from t2 to t3, the speed is v3 and the acceleration is a3 in the time period from t3 to t4.

[0061] When the electronic device performs speed-time planning, it needs to ensure that each joint of the master robot arm moves according to the planning result and can move to the desired position.

[0062] S103, determining the output torque of each joint of the master robot arm in the planning time period according to the speed-time planning data.

[0063] In this embodiment, for each joint of the master robot arm, the electronic device can use the dynamics model of the robot arm to determine the corresponding output torque according to the speed at each time point in the planning time period. Alternatively, the electronic device can determine the output torque of the joint at each time point according to the speed-time planning data through PD control.

[0064] S104, driving the motor of the joint of the master robot arm according to the output torque.

[0065] In this embodiment, the electronic device can convert the calculated output torque value of each joint of the master robot arm into a motor control signal, for example, a current, voltage or pulse width modulation (PWM) signal, and send it to the motor driver of the master robot arm through a communication interface. The motor driver drives the motor according to the received control signal to make the robot arm joint move according to the planning.

[0066] The electronic device can also continuously monitor the actual motion state of the robot arm, and if there is a deviation from the planning, it can also adjust the output torque or speed planning in real time according to the size and direction of the deviation to ensure that the robot arm can accurately and stably reach the desired position.

[0067] Alternatively, one motor can control one joint or multiple joints. When one motor controls multiple joints, the target torque can be determined according to the output torque of the multiple joints, and the corresponding motor can be controlled based on the target torque. This embodiment does not limit this.

[0068] The trajectory control method provided in the embodiment determines the output torque of the joint at each time point according to the planning result, and then controls the motor corresponding to the joint according to the output torque, so that the master robot arm joint moves to the desired position, and the position matching of the master robot arm and the slave robot arm is realized. Compared with the prior art, since the inverse dynamics model does not need to be constructed by obtaining the accurate parameters of the robot arm, the requirement for the computing power of the electronic device is reduced, the control difficulty of the electronic device is reduced, the calculation process is simple, the accuracy of the electronic device in controlling the speed of the master robot arm is improved, accurate trajectory tracking is realized, the problem of excessive speed or acceleration of the master robot arm during movement is avoided, and the safety of the master robot arm during movement is improved.

[0069] In some embodiments, the specific implementation of step S102 includes the following steps:

[0070] S201, determining the total distance of the master robot arm joint according to the desired position of the master robot arm joint and the current position of the master robot arm joint.

[0071] Specifically, for each joint of the master robot arm, the difference between the desired position and the current position is the total distance that the joint needs to move.

[0072] S202, determining the time length of each speed stage according to the preset speed planning algorithm and the total distance of the master robot arm joint.

[0073] Wherein, each speed stage includes an acceleration stage and a deceleration stage.

[0074] In the embodiment, the electronic device can select a suitable speed planning algorithm according to the performance parameters of the robot arm, for example, select S-type curve planning, trapezoidal speed planning and other algorithms according to the maximum acceleration, maximum deceleration, maximum speed and other parameters of the robot arm.

[0075] Based on the total distance of the master robot arm joint, the performance parameters of the robot arm and the selected speed planning algorithm, the time length proportion of each speed stage is determined, and then the time length of each speed stage is determined based on the total distance of the master robot arm displacement.

[0076] It should be understood that the desired speed of the master robot arm joint increases with time to the maximum speed acceptable by the robot arm in the acceleration stage, and gradually decreases to zero with time in the deceleration stage. If there is a uniform speed stage, the desired speed of the master robot arm joint remains the maximum speed acceptable by the robot arm.

[0077] Optionally, the specific manner of step S202 includes the following steps:

[0078] S2021, determine the time proportion of each speed stage according to the preset speed planning algorithm, the speed threshold and the acceleration threshold.

[0079] S2022, determine the time length of each speed stage according to the time proportion of each speed stage and the total path of the main machine arm joint.

[0080] Among them, the electronic device can determine the time proportion of each speed stage according to the time length of each speed stage, the maximum speed and the maximum acceleration of the mechanical arm.

[0081] Optionally, the time length proportion of each speed stage can be a fixed proportion, or a non-fixed proportion, for example, the time length proportion of the acceleration stage is 20% or 20%-30%. For the non-fixed proportion of time length, the electronic device can randomly select a value.

[0082] Taking the trapezoidal speed planning algorithm as an example, the total path of the main machine arm joint is equal to the area of the trapezoid surrounded by the speed-time curve and the coordinate axis. Based on this principle, the electronic device can determine the time length of each speed stage according to the time proportion of each speed stage and the total path of the main machine arm joint.

[0083] S203, determine the speed-time planning data of the main machine arm joint according to the total path of the main machine arm joint and the time length of each speed stage.

[0084] In this embodiment, the electronic device can generate a smooth speed-time curve based on the total path of the main machine arm joint and the calculated time length of each speed stage. It can be understood that the speed-time curve is used to describe the relationship between the speed and the time during the movement of the mechanical arm joint.

[0085] In the trajectory control method provided in this embodiment, the electronic device can efficiently plan and control the movement of the main machine arm joint, and ensure that the mechanical arm can operate accurately according to the expected path and speed.

[0086] In some embodiments, the specific implementation of step S104 includes the following steps:

[0087] S301, filter the output torque by using a low-pass filter.

[0088] In this embodiment, the electronic device can call a low-pass filter to process the output torque of the digital signal, so as to remove or weaken the noise and interference in the signal, while retaining or enhancing the effective output torque information.

[0089] S302, drive the motor of the main machine arm joint according to the filtered output torque.

[0090] Step S302 is Figure 2The step S104 in the embodiment is implemented in a similar manner, and details are not described herein.

[0091] In this embodiment, by filtering the output torque, noise interference can be reduced, and the control accuracy of the master robot arm can be further improved.

[0092] In some embodiments, the electronic device can further perform the following steps in the process of performing step S104:

[0093] S401, monitor the vibration frequency of the joint of the master robot arm.

[0094] S402, when the vibration frequency is greater than the frequency threshold, continue to drive the motor of the joint of the master robot arm according to the preset torque.

[0095] In this embodiment, when the vibration frequency of the joint of the master robot arm is high, the risk of injuring the patient will increase. Therefore, the electronic device can monitor the vibration frequency of each joint through the sensor during the process of driving the motor of the joint of the master robot arm.

[0096] When the vibration frequency is greater than the frequency threshold, it indicates that there is a high risk in the process of matching the position of the slave robot arm by the master robot arm. At this time, the electronic device can continue to drive the motor of the joint of the master robot arm according to the preset torque, so that the master robot arm can be safely moved to the desired position.

[0097] It should be understood that the preset torque is set under the premise of safety. Alternatively, the electronic device can select a suitable preset torque according to the mapping relationship between the torque and the vibration frequency, so as to ensure that the vibration frequency is less than or equal to the frequency threshold.

[0098] In this embodiment, during the movement of the master robot arm, the vibration frequency of each joint of the master robot arm is monitored, and when the vibration frequency is greater than the frequency threshold, the corresponding motor is driven according to the preset torque which can ensure safety, so as to reduce the vibration frequency of the joint of the master robot arm, effectively prevent the situation of excessive speed or acceleration, and improve the safety in the process of matching the position of the slave robot arm by the master robot arm.

[0099] In some embodiments, the step S101 of acquiring the desired position of the joint of the master robot arm comprises:

[0100] S501, acquire the current position of the joint of the slave robot arm.

[0101] Specifically, the electronic device acquires the current position of the joint of the slave robot arm currently controlled by the master robot arm.

[0102] The slave robot arm currently controlled by the master robot arm is determined according to the switching instruction. Here, the switching instruction is used to indicate switching the slave robot arm currently controlled by the master robot arm.

[0103] For example, the slave manipulator arms include a first slave manipulator arm and a second slave manipulator arm, the first slave manipulator arm is the slave manipulator arm currently controlled by the master manipulator arm. In this case, if the slave manipulator arm receives a switching instruction, the slave manipulator arm controlled by the master manipulator arm is switched from the first slave manipulator arm to the second slave manipulator arm, and the current position of the joint of the slave manipulator arm obtained by the electronic device is the current position of the joint of the second slave manipulator arm.

[0104] S502, according to the current position of the joint of the slave manipulator arm, the expected position of the joint of the master manipulator arm is determined through inverse solution calculation.

[0105] In this embodiment, the electronic device can obtain the current position of the slave manipulator arm through the sensor of the slave manipulator arm, and further determine the expected position of the joint of the master manipulator arm through inverse solution calculation.

[0106] In one embodiment, the electronic device is further connected with a side-kicking device, and the user can send a switching instruction to the slave manipulator arm through side-kicking.

[0107] For example, the slave manipulator arms include a first slave manipulator arm and a second slave manipulator arm, the first slave manipulator arm is the slave manipulator arm currently controlled by the master manipulator arm. In this case, if the slave manipulator arm receives a switching instruction, the slave manipulator arm controlled by the master manipulator arm is switched from the first slave manipulator arm to the second slave manipulator arm, and the current position of the joint of the slave manipulator arm obtained by the electronic device is the current position of the joint of the second slave manipulator arm.

[0108] When the endoscope is located at the second arm, the position information of the joint of the slave manipulator arm is obtained as the joint position information of the first arm and the third arm, and the arm number information is matched. It can be understood that at this time, the slave manipulator arm currently controlled by the master manipulator arm is the first arm and the third arm. The user's side-kicking can control the switching of the third arm and the fourth arm, at this time, the position information of the joint of the slave manipulator arm is obtained as the joint position information of the first arm and the fourth arm, and the arm number information is matched. At this time, the slave manipulator arm currently controlled by the master manipulator arm is the first arm and the fourth arm.

[0109] Similarly, when the endoscope is located at the third arm, the position information of the joint of the slave manipulator arm is obtained as the joint position information of the second arm and the fourth arm, and the arm number information is matched. It can be understood that at this time, the slave manipulator arm currently controlled by the master manipulator arm is the second arm and the fourth arm. The user's side-kicking can control the switching of the first arm and the second arm, at this time, the position information of the joint of the slave manipulator arm is obtained as the joint position information of the first arm and the fourth arm, and the arm number information is matched. At this time, the slave manipulator arm currently controlled by the master manipulator arm is the first arm and the fourth arm.

[0110] When the electronic device obtains the position information of the joint of the slave manipulator arm, if the arm number information changes, it can be determined that the slave manipulator arm currently controlled by the master manipulator arm changes.

[0111] In some embodiments, step S103 is specifically implemented by determining the output torque of the master arm joint at the next moment based on the actual position and actual speed of the master arm joint at the previous moment and the expected position and expected speed of the master arm joint at the next moment.

[0112] Specifically, the output torque can be calculated using formula (1).

[0113] (1)

[0114] in, For the output torque at the next moment;

[0115] K p Indicates the proportionality coefficient;

[0116] K v Indicates the damping coefficient;

[0117] x d Indicates the expected position at the next moment;

[0118] v d Indicates the expected velocity at the next moment;

[0119] x represents the actual position at the previous moment;

[0120] v represents the actual velocity at the previous moment.

[0121] In this embodiment, the electronic device combines the movement of the master arm joints at the previous moment to determine the output torque of the master arm joints at the next moment, which improves the stability of trajectory tracking, enables more precise control of the speed of the master arm joints, and reduces risks.

[0122] In some embodiments, the electronic device is a trajectory control system, including a time planning module, a speed planning module, a time monitoring module, and a torque calculation module.

[0123] The time planning module is used to obtain the total distance traveled by the joint of the master arm by the difference between the current position and the desired position of the joint, and then to determine the total time based on the planning results of the speed planning module, and allocate time for each speed stage.

[0124] The speed planning module is used to control the expected speed change pattern of the entire process, determine the time proportion of each speed stage, and then determine the specific time of each speed stage.

[0125] The time monitoring module is used to provide a time basis for speed planning when trajectory tracking is triggered.

[0126] The torque calculation module is used to calculate the output torque of the main robotic arm joints in real time and send it to the corresponding drive motor.

[0127] Figure 3 Fig. 1 shows a structural schematic diagram of a trajectory control device according to an embodiment of the present application. As shown in Fig. 1, the trajectory control device 10 according to the embodiment of the present application is used to implement the operation corresponding to the electronic device in any of the method embodiments described above. The trajectory control device 10 according to the embodiment of the present application comprises: Figure 3

[0128] The acquisition module 11 is configured to acquire the desired position of the master robot arm joint and the current position of the master robot arm joint.

[0129] The determination module 12 is configured to determine the speed-time planning data of the master robot arm joint according to the desired position of the master robot arm joint and the current position of the master robot arm joint.

[0130] The determination module 12 is further configured to determine the output torque of the master robot arm joint within the planning time length according to the speed-time planning data.

[0131] The driving module 13 is configured to drive the motor of the master robot arm joint according to the output torque.

[0132] The trajectory control device 10 according to the embodiment of the present application can execute the method embodiments described above. The specific implementation principles and technical effects can be referred to the method embodiments described above, and will not be described here again.

[0133] Figure 4 Fig. 2 shows a hardware structural schematic diagram of an electronic device according to an embodiment of the present application. As shown in Fig. 2, the electronic device 20 according to the embodiment of the present application is used to implement the operation corresponding to the electronic device in any of the method embodiments described above. The electronic device 20 according to the embodiment of the present application can comprise a memory 21, a processor 22 and a communication interface 24. Figure 4 The memory 21 is configured to store a computer program. The memory 21 can include a high-speed Random Access Memory (RAM), and can also include a Non-Volatile Memory (NVM), for example, at least one disk memory, and can also be a U disk, a mobile hard disk, a read-only memory, a magnetic disk or an optical disk, etc.

[0134]

[0135] ​​The processor 22 is configured to execute the computer program stored in the memory to implement the method in the above embodiments. Details can be referred to the description of the method embodiments. The processor 22 can be a central processing unit (CPU), and can also be other general-purpose processors, digital signal processors (DSP), application specific integrated circuits (ASIC), etc. The general-purpose processor can be a microprocessor or the processor can also be any conventional processor and the like. The steps of the method disclosed in the application can be directly embodied as the execution of the hardware processor, or the execution of the combination of hardware and software modules in the processor.

[0136] Optionally, the memory 21 can be independent or integrated with the processor 22.

[0137] When the memory 21 is a device independent of the processor 22, the electronic device 20 can further include a bus 23. The bus 23 is configured to connect the memory 21 and the processor 22. The bus 23 can be an industry standard architecture (ISA) bus, a peripheral component interconnect (PCI) bus, an extended industry standard architecture (EISA) bus, or the like. The bus can be divided into an address bus, a data bus, a control bus, and the like. For the convenience of representation, the bus in the drawings of the present application does not limit to only one bus or one type of bus.

[0138] The communication interface 24 can be connected with the processor 22 through the bus 23. The processor 22 can control the communication interface 24 to realize the functions of receiving and sending signals.

[0139] The electronic device 20 provided in the embodiment can be used to execute the trajectory control method described above, and the implementation manner and technical effects are similar, which will not be described here.

[0140] The present application also provides a computer readable storage medium, and the computer readable storage medium stores computer programs / instructions. When the computer programs / instructions are executed by the processor, the computer programs / instructions are used to implement the method provided in the various embodiments.

[0141] The computer readable storage medium can be a computer readable storage medium or a communication medium. The communication medium includes any medium that facilitates transfer of a computer program from one place to another. A storage medium can be any available medium that can be accessed by a general purpose or special purpose computer. By way of example, and not limitation, computer readable storage medium can be RAM, ROM, EEPROM, EPROM, flash memory, floppy disk, etc. Computer readable storage medium does not comprise a propagating signal.

[0142] In particular, the computer readable storage medium can be realized by any type of volatile or non-volatile storage devices, or a combination thereof, such as a Static Random-Access Memory (SRAM), an Electrically-Erasable Programmable Read-Only Memory (EEPROM), an Erasable Programmable Read Only Memory (EPROM), a Programmable Read-Only Memory (PROM), a Read-Only Memory (ROM), a magnetic storage, a flash memory, a magnetic disk, or an optical disk. The storage medium can be any available medium that can be accessed by a general purpose or special purpose computer.

[0143] The present application also provides a computer program product including a computer program / instruction stored in a computer readable storage medium. At least one processor of a device can read the computer program / instruction from the computer readable storage medium, and the at least one processor executes the computer program / instruction to cause the device to implement the method provided by the various embodiments described above.

[0144] In several embodiments provided in the present application, it should be understood that the disclosed apparatus and method can be implemented by other manners. For example, the apparatus embodiments described above are only illustrative, for example, the division of the modules is only a logical function division, and actual implementation can have another division manner, for example, a plurality of modules can be combined or integrated into another system, or some features can be ignored or not executed. In addition, the coupling or direct coupling or communication connection between the displayed or discussed modules can be indirect coupling or communication connection through some interfaces, apparatuses or modules, and can be electrical, mechanical or other forms.

[0145] Each module can be physically separated, for example, installed at different positions of one device, or installed on different devices, or distributed on a plurality of network elements, or distributed on a plurality of processors. Each module can also be integrated together, for example, installed in the same device, or integrated in a set of codes. Each module can exist in the form of hardware, or can exist in the form of software, or can be realized in the form of software plus hardware. The present application can select some or all modules to achieve the purpose of the embodiment scheme according to actual needs.

[0146] It should be understood that although each step in the flowchart in the above embodiment is displayed in sequence according to the arrow, these steps are not necessarily executed in sequence according to the arrow. Unless otherwise stated herein, the execution of these steps has no strict sequence limitation, and they can be executed in other orders. Moreover, at least part of the steps in the figure can include a plurality of sub-steps or stages, which are not necessarily executed at the same time, but can be executed at different times, and the execution order is not necessarily sequential, but can be executed in rotation or alternation with other steps or sub-steps or stages of other steps.

[0147] Finally, it should be pointed out that: the above embodiments are only used to illustrate the technical solutions of the present application, and not to limit them. Although the present application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that they can still modify the technical solutions recorded in the foregoing embodiments, or make equivalent replacement for some or all of the technical features. Such modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of the embodiments of the present application.

Claims

1. A trajectory control method, characterized in that, The method includes: Obtain the current position of the slave manipulator joint currently controlled by the master manipulator and the current position of the master manipulator joint. Based on the current position of the slave manipulator joint, determine the desired position of the master manipulator joint. Based on the desired position and current position of the master arm joint, the total distance traveled by the master arm joint is determined; and based on a preset speed planning algorithm and the total distance traveled by the master arm joint, the duration of each speed stage is determined, wherein each speed stage includes an acceleration stage and a deceleration stage; based on the total distance traveled by the master arm joint and the duration of each speed stage, speed-time planning data for the master arm joint is determined. Based on the actual position and speed of the master arm joint at the previous moment, and the expected position and speed of the master arm joint at the next moment, determine the output torque of the master arm joint at the next moment. The motor that drives the joints of the main robotic arm is based on the output torque.

2. The method according to claim 1, characterized in that, Based on the preset speed planning algorithm and the total distance traveled by the main robotic arm joints, the duration of each speed phase is determined, including: The time percentage of each speed stage is determined based on the preset speed planning algorithm, speed threshold, and acceleration threshold. The duration of each speed phase is determined based on the time percentage of each speed phase and the total distance traveled by the main robotic arm joints.

3. The method according to claim 1 or 2, characterized in that, The motor that drives the joints of the main robotic arm according to the output torque includes: The output torque is filtered using a low-pass filter; The motors driving the joints of the master robotic arm are based on the filtered output torque.

4. The method according to claim 1 or 2, characterized in that, The process of driving the motor of the main hand robotic arm joint according to the output torque includes: Monitor the vibration frequency of the joints of the master robotic arm; When the vibration frequency is greater than the frequency threshold, the motor of the main hand robotic arm joint continues to drive according to the preset torque.

5. The method according to claim 1 or 2, characterized in that, Determining the desired position of the master arm joint based on the current position of the slave arm joint includes: Based on the current position of the slave manipulator joint, the desired position of the master manipulator joint is determined by inverse kinematics calculation.

6. The method according to claim 1, characterized in that, Obtain the current position of the robotic arm joints, including: The current position of the slave robotic arm joint currently controlled by the master robotic arm is obtained. The slave robotic arm currently controlled by the master robotic arm is determined according to a switching instruction, which is used to indicate the switching of the slave robotic arm currently controlled by the master robotic arm.

7. A trajectory control device, characterized in that, include: The acquisition module is used to acquire the current position of the slave manipulator joint currently controlled by the master manipulator and the current position of the master manipulator joint, and to determine the desired position of the master manipulator joint based on the current position of the slave manipulator joint. The determination module is used to determine the total distance traveled by the main hand robotic arm joint based on the desired position of the main hand robotic arm joint and the current position of the main hand robotic arm joint. Based on a preset speed planning algorithm and the total distance traveled by the main hand robotic arm joint, the duration of each speed stage is determined, wherein each speed stage includes an acceleration stage and a deceleration stage; based on the total distance traveled by the main hand robotic arm joint and the duration of each speed stage, speed-time planning data for the main hand robotic arm joint is determined. The determining module is also used to determine the output torque of the master arm joint at the next moment based on the actual position and actual speed of the master arm joint at the previous moment and the expected position and expected speed of the master arm joint at the next moment. The drive module is used to drive the motors of the main hand robotic arm joints according to the output torque.

8. An electronic device, characterized in that, include: Memory and processor; The memory is used to store computer programs; The processor is used to execute the computer program stored in the memory to implement the trajectory control method as described in any one of claims 1-6.

9. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program, which, when executed by a processor, is used to implement the trajectory control method as described in any one of claims 1-6.

10. A computer program product, characterized in that, The computer program product includes a computer program that, when executed by a processor, implements the trajectory control method according to any one of claims 1-6.

Images