Track control method and device

Abstract

The invention provides a track control method and equipment. The method comprises the steps that the expected position and the current position of a master manipulator mechanical arm joint are obtained; according to the expected position of the master manipulator mechanical arm joint and the current position of the master manipulator mechanical arm joint, determining speed-time planning data of the master manipulator mechanical arm joint; according to the speed-time planning data, the output torque of a main manipulator mechanical arm joint within the planning duration is determined; and the motor drives the main manipulator mechanical arm joint according to the output torque. The method is used for achieving the effect of improving the master manipulator control precision.

Description

Technical Field

[0001] This application relates to the field of medical devices, and more particularly to a trajectory control method and device. Background Technology

[0002] When the slave hand of a surgical robot exceeds its limits or when switching slave hands, the position of the master hand needs to be redefined based on the position of the slave hand to avoid injury to the patient. Trajectories of the master hand need to be tracked during the repositioning process.

[0003] In related technologies, when tracking the trajectory of the master arm, the control system plans the motion trajectory of the master arm joints according to task requirements, including parameters such as position, velocity, and acceleration. Using an inverse dynamics model, it calculates the required joint torque or force based on the planned trajectory and employs a preset proportional-derivative (PD) controller to adjust the deviation between the actual motion and the desired trajectory. 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, which is sent to the robot actuator. The robot executes the control command and uses sensors to provide feedback on the actual motion state for the next control cycle.

[0004] However, the above trajectory control methods require high computing power from the control system, resulting in low speed control accuracy of the master. Summary of the Invention

[0005] This application provides a trajectory control method and device to solve the problem of low control accuracy of the master hand in the existing trajectory control methods.

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

[0007] Obtain the desired position and current position of the master arm joints;

[0008] Based on the desired position and current position of the master arm joints, determine the speed-time planning data for the master arm joints;

[0009] Based on the speed-time planning data, determine the output torque of the master robotic arm joints within the planned duration;

[0010] The motor that drives the joints of the main robotic arm is determined by the output torque.

[0011] In some embodiments, based on the desired position and the current position of the master arm joint, velocity-time planning data for the master arm joint is determined, including:

[0012] The total distance traveled by the master arm joints is determined based on the desired position of the master arm joints and their current position.

[0013] Based on the preset speed planning algorithm and the total distance of the master robotic arm joints, the duration of each speed stage is determined, where each speed stage includes an acceleration stage and a deceleration stage.

[0014] Based on the total distance traveled by the master arm joints and the duration of each speed phase, determine the speed-time planning data for the master arm joints.

[0015] In some embodiments, the duration of each speed phase is determined based on a preset speed planning algorithm and the total distance traveled by the master robotic arm joints, including:

[0016] The time percentage of each speed stage is determined based on the preset speed planning algorithm, speed threshold, and acceleration threshold.

[0017] The duration of each speed phase is determined based on the time percentage of each speed phase and the total distance traveled by the master robotic arm joints.

[0018] In some embodiments, the motor driving the joints of the master robotic arm according to the output torque includes:

[0019] A low-pass filter is used to filter the output torque;

[0020] The motors driving the joints of the master robotic arm are based on the filtered output torque.

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

[0022] Monitor the vibration frequency of the joints of the master robotic arm;

[0023] When the vibration frequency exceeds the frequency threshold, the motor of the master robotic arm joint continues to drive according to the preset torque.

[0024] In some embodiments, obtaining the desired position of the master arm joint includes:

[0025] Get the current position of the joints of the robotic arm;

[0026] Based on the current position of the slave arm joint, the desired position of the master arm joint is determined by inverse kinematics calculation.

[0027] In some embodiments, obtaining the current position from the joint of the robotic arm includes:

[0028] Obtain the current position of the slave robot joint currently controlled by the master robot arm. The slave robot currently controlled by the master robot arm is determined according to the switching command, which is used to indicate the switching of the slave robot arm currently controlled by the master robot arm.

[0029] In some embodiments, determining the output torque of the master arm joints within a planned duration based on velocity-time planning data includes:

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

[0031] Secondly, this application provides a trajectory control device, comprising:

[0032] The acquisition module is used to acquire the desired position and the current position of the main hand robotic arm joints.

[0033] The determination module is used to determine the speed-time planning data of the master arm joints based on the desired position and the current position of the master arm joints.

[0034] The determination module is also used to determine the output torque of the master arm joints within the planned duration based on speed-time planning data;

[0035] The drive module is used to drive the motors of the main robotic arm joints according to the output torque.

[0036] Thirdly, this application provides an electronic device, including: a memory and a processor;

[0037] The memory is used to store computer programs; the processor is used to execute the computer programs stored in the memory to implement the trajectory control method of the first aspect and any embodiment of the first aspect.

[0038] Fourthly, this application provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the trajectory control method of the first aspect and any embodiment of the first aspect.

[0039] Fifthly, this application provides a computer program product, which includes a computer program that, when executed by a processor, implements the trajectory control method of the first aspect and any embodiment of the first aspect.

[0040] The trajectory control method and device provided in this application determine the speed-time planning data of the master arm joint based on the desired position and current position of the master arm joint, and then determine the output torque of the master arm joint within the planning time to drive the motor of the master arm joint. This reduces the computing power pressure of the master arm control system and improves the speed control accuracy of the master arm. Attached Figure Description

[0041] To more clearly illustrate the technical solutions in this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0042] Figure 1 A schematic diagram of a trajectory control scenario provided in an embodiment of this application;

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

[0044] Figure 3 This is a schematic diagram of the structure of a trajectory control device provided in an embodiment of this application;

[0045] Figure 4 This is a schematic diagram of the hardware structure of an electronic device provided in an embodiment of this application. Detailed Implementation

[0046] To make the objectives, technical solutions, and advantages of this application clearer, the technical solutions of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0047] Currently, robots typically employ PD (Proportional-Integral-Derivative) and PID (Proportional-Integral-Derivative) control algorithms. For example, fixed-point control often uses the PD control algorithm, which provides good control performance. Compared to fixed-point control, trajectory control places higher demands on the control algorithm. It requires close monitoring of changes in the robot's speed and acceleration; otherwise, excessive speed or acceleration could damage the robot or even cause serious personal injury accidents.

[0048] In related technologies, trajectory control methods mainly include the following: First, combining inverse dynamics models and PD controllers. Specifically, the control system plans the robot's motion trajectory based on task requirements, including parameters such as position, velocity, and acceleration. Using the inverse dynamics model, the required joint torques or forces are calculated based on the planned trajectory. A preset PD controller is used to adjust the deviation between the actual motion and the desired trajectory. 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, which is sent to the robot's actuator. The robot executes the control command and uses sensors to provide feedback on the actual motion state for the next control cycle. Second, combining inverse dynamics models and variable PD controllers. Building upon the first method, the parameters of the PD controller are dynamically adjusted according to the actual situation to improve control performance. Third, combining feedback linearization, PD controllers, and feedforward compensation. Specifically, the control system converts the robot's nonlinear dynamic model into a linear model through state transformation or nonlinear control strategies, simplifying control design. A PD controller is designed on the linearized system to handle deviations. Feedforward compensation using the inverse dynamics model further improves tracking accuracy. The output of the PD controller is combined with the feedforward compensation signal to form a control command and execute it, while feedback is collected for subsequent control. Fourth, by combining feedback linearization, PID controller and feedforward compensation, an integral term (I) is introduced on the basis of the third method to form a PID controller, so as to eliminate the steady-state error of the system and improve the control accuracy.

[0049] The trajectory control methods described above all use inverse dynamics models as feedforwards. This requires obtaining accurate robot system parameters to establish the inverse dynamics model. However, for complex robot systems, obtaining accurate parameters is difficult, leading to increased time costs and control difficulties. In particular, for robots with complex systems and weak computing power, the above methods have obvious shortcomings.

[0050] To address the aforementioned problems, this application proposes a trajectory control method and device. In this method, based on the desired position and current position of the master arm joints, velocity-time planning is performed on the master arm joints, and the master arm joints are controlled to move to the desired position based on the planning results. This method eliminates the need for an inverse dynamics model, saving time and reducing computational complexity. It also lowers the computational requirements of the control system and improves the accuracy of velocity and acceleration control of the master arm joints.

[0051] The technical solutions of this 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 again in some embodiments.

[0052] Figure 1 This illustration shows a scenario diagram of trajectory control according to an embodiment of this application. Figure 1 As shown, the surgical robot includes a movable control console and a slave robotic arm. The end effector of the robotic arm is typically connected to a multi-jointed actuator and a trocar. The actuator connects surgical instruments, and the trocar provides a channel for the instruments, enabling single-port minimally invasive surgery. When changing surgical instruments or when an instrument's travel exceeds its limit, the slave robotic arm must be kept in its original position while the master robotic arm on the control console is moved to realign with the slave robotic arm, preventing injury to the patient on the slave side.

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

[0054] Figure 2 A flowchart of a trajectory control method provided in an embodiment of this application is shown. Figure 2 As shown, with an electronic device as the execution subject, the method in this embodiment may include the following steps:

[0055] S101. Obtain the desired position and current position of the master arm joint.

[0056] In this embodiment, the electronic device can establish a stable communication connection with the master robotic arm via bus or wireless means to ensure that data can be acquired and sent in real time.

[0057] The master robotic arm consists of multiple joints. For each joint, its current position and desired position need to be obtained. Electronic devices can obtain the current position of each joint through the position sensors of the master robotic arm and the current position of the slave robotic arm through the position sensors of the slave robotic arm. Based on the position of the slave robotic arm, the desired positions of each joint of the master robotic arm are determined.

[0058] S102. Determine the speed-time planning data of the master arm joints based on the desired position and the current position of the master arm joints.

[0059] In this embodiment, the electronic device can determine the distance or angle that the joint needs to move based on the desired position and current position of the main hand robotic arm joint, and generate speed-time planning data for each joint using algorithms such as S-curve planning and polynomial interpolation based on the distance or angle to be moved, the dynamic characteristics of the robotic arm, and path smoothness requirements.

[0060] The planning results include the velocity and acceleration of each joint at each time point. For example, the velocity of joint A is v1 and the acceleration is a1 during the time period t1~t2, the velocity is v2 and the acceleration is a2 during the time period t2~t3, and the velocity is v3 and the acceleration is a3 during the time period t3~t4.

[0061] When electronic devices perform speed-time planning, it is necessary to ensure that each joint of the master robotic arm moves according to the planning results and can move to the desired position.

[0062] S103. Based on the speed-time planning data, determine the output torque of the master robotic arm joints within the planned duration.

[0063] In this embodiment, for each joint of the master robotic arm, at each moment within the planned time period, the electronic device can utilize the robotic arm's dynamic model to determine the corresponding output torque based on the velocity at that moment. Optionally, the electronic device can use PD control to determine the joint's output torque at each moment based on velocity-time planning data.

[0064] S104. The motor that drives the joints of the main robotic 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 robotic arm into a motor control signal, such as a current, voltage, or pulse width modulation (PWM) signal, and send it to the motor driver of the master robotic arm through a communication interface. The motor driver drives the motor according to the received control signal, causing the robotic arm joints to move according to the plan.

[0066] The electronic equipment can also continuously monitor the actual movement status of the robotic arm. If there is a deviation from the plan, it can adjust the output torque or speed plan in real time according to the magnitude and direction of the deviation to ensure that the robotic arm can accurately and stably reach the desired position.

[0067] Optionally, a motor can control one joint or multiple joints. When a motor controls multiple joints, the target torque can be determined based on the output torque of the multiple joints, and the corresponding motor can be controlled based on the target torque. This embodiment does not impose any restrictions on this.

[0068] The trajectory control method provided in this embodiment performs velocity-time planning on the master robotic arm joints based on their current desired position. The output torque of the joints at various times is determined according to the planning results, and the corresponding motors are then controlled based on this output torque to move the master robotic arm joints to the desired position, achieving position matching between the master and slave robotic arms. Compared to existing technologies, this method reduces the computational requirements of electronic devices and simplifies control, as it eliminates the need for precise parameters of the robotic arm to construct an inverse dynamics model. The simplified calculation process improves the accuracy of the electronic device's control over the master robotic arm's speed, achieving precise trajectory tracking and avoiding excessive speed or acceleration during master robotic arm movement, thus enhancing safety.

[0069] In some embodiments, step S102 is specifically implemented by the following steps:

[0070] S201. Determine the total distance traveled by the master arm joints based on the desired position of the master arm joints and the current position of the master arm joints.

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

[0072] S202. Determine the duration of each speed stage based on the preset speed planning algorithm and the total distance of the master robotic arm joints.

[0073] Each speed phase includes an acceleration phase and a deceleration phase.

[0074] In this embodiment, the electronic device can select a suitable speed planning algorithm based on the performance parameters of the robotic arm. For example, it can select algorithms such as S-curve planning and trapezoidal speed planning based on parameters such as the maximum acceleration, maximum deceleration, and maximum speed of the robotic arm.

[0075] Based on the total distance traveled by the main arm joints, the performance parameters of the main arm, and the selected speed planning algorithm, the duration of each speed stage is determined, and then the duration of each speed stage is determined based on the total distance traveled by the main arm displacement.

[0076] It should be understood that the desired speed of the master arm's joints increases over time during the acceleration phase to reach the maximum acceptable speed of the robotic arm, and gradually decreases to zero over time during the deceleration phase. If there is a constant speed phase, the desired speed of the master arm's joints remains at the maximum acceptable speed of the robotic arm.

[0077] Optionally, step S202 may include the following steps:

[0078] S2021. Determine the time percentage of each speed stage based on the preset speed planning algorithm, speed threshold, and acceleration threshold.

[0079] S2022. Determine the duration of each speed stage based on the time percentage of each speed stage and the total distance traveled by the main robotic arm joints.

[0080] The electronic device can determine the time percentage of each speed stage based on the duration of each speed stage, the maximum speed of the robotic arm, and the maximum acceleration.

[0081] Optionally, the duration of each speed phase can be a fixed percentage or a non-fixed percentage. For example, the duration of the acceleration phase can be 20% or 20% to 30%. For non-fixed percentages, the electronic device can randomly select a value.

[0082] Taking the trapezoidal velocity planning algorithm as an example, the total distance traveled by the main robotic arm joints is equal to the area of ​​the trapezoid formed by the velocity-time curve and the coordinate axes. Based on this principle, electronic devices can determine the duration of each velocity stage according to the time proportion of each velocity stage and the total distance traveled by the main robotic arm joints.

[0083] S203. Determine the speed-time planning data for the master arm joints based on the total distance traveled by the master arm joints and the duration of each speed phase.

[0084] In this embodiment, the electronic device can calculate the duration of each speed stage based on the total distance traveled by the master robotic arm joints, generating a smooth speed-time curve. It can be understood that the speed-time curve describes the relationship between the speed of the robotic arm joints and time during movement.

[0085] In the trajectory control method provided in this embodiment, the electronic device can efficiently plan and control the movement of the master robotic arm joints, ensuring that the robotic arm can perform precise operations according to the desired path and speed.

[0086] In some embodiments, step S104 is specifically implemented by the following steps:

[0087] S301. Low-pass filtering is used to filter the output torque.

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

[0089] S302. Drive the motor of the main arm joint based on the filtered output torque.

[0090] Step S302 and Figure 2The implementation of step S104 in the embodiment is similar, and will not be repeated here.

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

[0092] In some embodiments, during the execution of step S104, the electronic device may also perform the following steps:

[0093] S401. Monitor the vibration frequency of the master robotic arm joints.

[0094] S402. When the vibration frequency is greater than the frequency threshold, the motor of the main arm joint continues to drive according to the preset torque.

[0095] In this embodiment, the risk of injury to the patient increases when the vibration frequency of the main robotic arm joints is high. Therefore, the electronic device can monitor the vibration frequency of each joint through sensors during the process of driving the main robotic arm joint motors.

[0096] When the vibration frequency exceeds the frequency threshold, it indicates a significant risk during the matching process between the master robotic arm and the slave robotic arm. In this case, the electronic equipment can continue to drive the motors of the master robotic arm joints with a preset torque, enabling the master robotic arm to safely move to the desired position.

[0097] It should be understood that the preset torque is set under the premise of ensuring safety. Optionally, the electronic device can select an appropriate preset torque based on the mapping relationship between torque and vibration frequency 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 robotic arm, the vibration frequency of each joint of the master robotic arm is monitored. When the vibration frequency is greater than the frequency threshold, the corresponding motor is driven according to a preset torque that can ensure safety, thereby reducing the vibration frequency of the joints of the master robotic arm, effectively curbing the occurrence of excessive speed or acceleration, and improving the safety of the master robotic arm matching the position of the slave robotic arm.

[0099] In some embodiments, obtaining the desired position of the master arm joint in step S101 includes:

[0100] S501. Obtain the current position of the joint of the robotic arm.

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

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

[0103] For example, a slave robotic arm includes a first slave robotic arm and a second slave robotic arm, where the first slave robotic arm is currently controlled by the master robotic arm. In this case, if the slave robotic arm receives a switching command, it will switch the slave robotic arm controlled by the master robotic arm from the first slave robotic arm to the second slave robotic arm. Then, the current position of the slave robotic arm joint obtained by the electronic device is the current position of the second slave robotic arm joint.

[0104] S502. Based on the current position of the slave manipulator joint, determine the desired position of the master manipulator joint through inverse kinematics calculation.

[0105] In this embodiment, the electronic device can obtain the current position of the robotic arm from the sensors of the robotic arm, and then determine the desired position of the joint of the master robotic arm through inverse kinematics calculation.

[0106] In one embodiment, the electronic device is also connected to a side-kick device, which allows the user to send a switching command to the slave robotic arm by side-kicking.

[0107] For example, the slave robotic arm includes arm 1, arm 2, arm 3, and arm 4. Among them, arm 1 and arm 2 are controlled by the first master robotic arm, and arm 3 and arm 4 are controlled by the second master robotic arm.

[0108] When the endoscope is positioned at arm 2, the position information of the slave robotic arm joints is obtained by acquiring the joint position information of arms 1 and 3 and matching it with the arm number information. This means that at this time, the master robotic arm is currently controlling slave robotic arms 1 and 3. When the user side-kicks, they can switch between arms 3 and 4. In this case, the position information of the slave robotic arm joints is obtained by acquiring the joint position information of arms 1 and 4 and matching it with the arm number information. At this time, the master robotic arm is currently controlling slave robotic arms 1 and 4.

[0109] Similarly, when the endoscope is positioned at arm 3, obtaining the position information of the slave robotic arm joints involves obtaining the joint position information of arms 2 and 4, and matching them with the arm number information. This means that at this time, the master robotic arm is currently controlling slave robotic arms 2 and 4. When the user side-kicks, they can switch between arms 1 and 2. In this case, obtaining the position information of the slave robotic arm joints involves obtaining the joint position information of arms 1 and 4, and matching them with the arm number information. At this time, the master robotic arm is currently controlling slave robotic arms 1 and 4.

[0110] When an electronic device acquires the position information of a slave robotic arm joint, if the arm number information changes, it can determine that the slave robotic arm currently controlled by the master robotic arm has changed.

[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 A schematic diagram of the structure of a trajectory control device according to an embodiment of this application is shown, as follows: Figure 3 As shown, the trajectory control device 10 of this embodiment is used to implement the operation corresponding to the electronic device in any of the above method embodiments. The trajectory control device 10 of this embodiment includes:

[0128] The acquisition module 11 is used to acquire the desired position and the current position of the main hand robotic arm joints;

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

[0130] The determination module 12 is also used to determine the output torque of the master arm joints within the planned duration based on the speed-time planning data;

[0131] Drive module 13 is used to drive the motors of the main hand robotic arm joints according to the output torque.

[0132] The trajectory control device 10 provided in this application embodiment can execute the above method embodiment. Its specific implementation principle and technical effect can be found in the above method embodiment, and will not be repeated here.

[0133] Figure 4 A schematic diagram of the hardware structure of an electronic device according to an embodiment of this application is shown. Figure 4 As shown, the electronic device 20 is used to implement the operation corresponding to the electronic device in any of the above method embodiments. The electronic device 20 in this embodiment may include: a memory 21, a processor 22, and a communication interface 24.

[0134] The memory 21 is used to store computer programs. The memory 21 may include high-speed random access memory (RAM) and may also include non-volatile memory (NVM), such as at least one disk storage device, and may also be a USB flash drive, external hard drive, read-only memory, disk or optical disc, etc.

[0135] Processor 22 is used to execute computer programs stored in memory to implement the methods in the above embodiments. For details, please refer to the relevant descriptions in the foregoing method embodiments. The processor 22 can be a Central Processing Unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), etc. A general-purpose processor can be a microprocessor or any conventional processor. The steps of the methods disclosed in this invention can be directly implemented by a hardware processor, or implemented by a combination of hardware and software modules within the processor.

[0136] Alternatively, the memory 21 can be either standalone or integrated with the processor 22.

[0137] When the memory 21 is a device independent of the processor 22, the electronic device 20 may also include a bus 23. This bus 23 is used to connect the memory 21 and the processor 22. The bus 23 may be an Industry Standard Architecture (ISA) bus, a Peripheral Component Interconnect (PCI) bus, or an Extended Industry Standard Architecture (EISA) bus, etc. Buses can be categorized as address buses, data buses, control buses, etc. For ease of illustration, the buses shown in the accompanying drawings are not limited to a single bus or a single type of bus.

[0138] The communication interface 24 can be connected to the processor 22 via 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 this embodiment can be used to execute the trajectory control method described above. Its implementation method and technical effect are similar, and will not be described again in this embodiment.

[0140] This application also provides a computer-readable storage medium storing a computer program / instructions, which, when executed by a processor, are used to implement the methods provided in the various embodiments described above.

[0141] The computer-readable storage medium can be a computer storage medium or a communication medium. A communication medium includes any medium that facilitates the transfer of a computer program from one location to another. A computer storage medium can be any available medium accessible to a general-purpose or special-purpose computer. For example, a computer-readable storage medium is coupled to a processor, enabling the processor to read information from and write information to the computer-readable storage medium. Of course, the computer-readable storage medium can also be a component of the processor. The processor and the computer-readable storage medium can reside in an Application Specific Integrated Circuit (ASIC). Alternatively, the ASIC can reside in a user equipment. Of course, the processor and the computer-readable storage medium can also exist as discrete components in a communication device.

[0142] Specifically, the computer-readable storage medium can be implemented by any type of volatile or non-volatile storage device or a combination thereof, such as Static Random-Access Memory (SRAM), Electrically Erasable Programmable Read-Only Memory (EEPROM), Erasable Programmable Read Only Memory (EPROM), Programmable Read-Only Memory (PROM), Read-Only Memory (ROM), magnetic storage, flash memory, magnetic disk, or optical disk. The storage medium can be any available medium accessible to general-purpose or special-purpose computers.

[0143] This application also provides a computer program product comprising a computer program / instructions stored in a computer-readable storage medium. At least one processor of the device can read the computer program / instructions from the computer-readable storage medium, and the at least one processor executes the computer program / instructions to cause the device to perform the methods provided in the various embodiments described above.

[0144] In the several embodiments provided in this application, it should be understood that the disclosed apparatus and methods can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative; for instance, the division of modules is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple modules may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between apparatuses or modules may be electrical, mechanical, or other forms.

[0145] The modules can be physically separate, for example, installed in different locations within a single device, installed on different devices, distributed across multiple network units, or distributed across multiple processors. Alternatively, the modules can be integrated, for example, installed in the same device, or integrated into a single codebase. The modules can exist in hardware form, software form, or a combination of both. This application can select some or all of the modules to achieve the objectives of this embodiment based on actual needs.

[0146] It should be understood that although the steps in the flowcharts of the above embodiments are shown sequentially according to the arrows, these steps are not necessarily executed in the order indicated by the arrows. Unless explicitly stated herein, there is no strict order restriction on the execution of these steps, and they can be executed in other orders. Moreover, at least some of the steps in the figures may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily completed at the same time, but can be executed at different times, and their execution order is not necessarily sequential, but can be performed alternately or in turn with other steps or at least some of the sub-steps or stages of other steps.

[0147] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of this application, and not to limit them. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features. These modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of this application.

Claims

1. A trajectory control method, characterized in that, The method includes: Obtain the desired position and current position of the master arm joints; Based on the desired position of the master arm joint and the current position of the master arm joint, determine the speed-time planning data of the master arm joint; Based on the speed-time planning data, determine the output torque of the master robotic arm joints within the planned duration; 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 desired position and current position of the master arm joint, determine the velocity-time planning data for the master arm joint, including: The total distance traveled by the main hand robotic arm joint is determined 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 the preset speed planning algorithm and the total distance of 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 phase, the speed-time planning data for the main hand robotic arm joint is determined.

3. The method according to claim 2, 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.

4. The method according to any one of claims 1-3, 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.

5. The method according to any one of claims 1-3, 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.

6. The method according to any one of claims 1-3, characterized in that, Obtain the desired positions of the master arm joints, including: Get the current position of the joints of the robotic arm; Based on the current position of the slave manipulator joint, the desired position of the master manipulator joint is determined by inverse kinematics calculation.

7. The method according to claim 6, 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.

8. The method according to any one of claims 1-3, characterized in that, Based on the speed-time planning data, determine the output torque of the master robotic arm joints within the planned duration, including: 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.

9. A trajectory control device, characterized in that, include: The acquisition module is used to acquire the desired position and the current position of the main hand robotic arm joints. The determination module is used to determine the speed-time planning data of 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. The determining module is also used to determine the output torque of the main hand robotic arm joint within the planned duration based on the speed-time planning data; The drive module is used to drive the motors of the main hand robotic arm joints according to the output torque.

10. 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-8.

11. 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-8.

12. 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-8.

Images