Double-arm control method and system of nursing robot
Through the two-arm control method and system, the problem of poor coordination and coordination of multi-arm cooperation of nursing robots is solved, and higher operating accuracy and safety are achieved, and nursing operations in different environments are adapted to nursing operations in different environments.
Patent Information
- Application Number
- CN202411090773.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-08
- Publication Date
- 2024-11-19
- Estimated Expiration
- Not applicable · inactive patent
AI Technical Summary
The multi-arm collaboration of existing nursing robots is poorly coordinated in the same space and cannot be compatible with operations in different environments, resulting in insufficient operational accuracy and safety.
Using a two-arm control method and system, the two-arm movement trajectory of the nursing robot is collected, and the active arm and the driven arm are defined. Dynamic control is carried out based on the fuzzy PID control algorithm, and the attitude coordination relationship is constructed, contact parameters and spatial location are collected, and nursing is defined. Path, establish a mechanical system to control the nursing logic of both arms.
Improve the operation accuracy and safety of nursing robots, enhance the coordination of both arms, and can effectively perform coordinated operations in different environments. The combination of fuzzy PID control algorithm and radial basis function neural network improves the environmental adaptation of the system. and response speed.
Smart Images

Figure CN118977241A_ABST
Abstract
Description
Technical Field
[0001] The present application relates to the field of intelligent robots, and in particular to a dual-arm control method and system for a nursing robot. Background Art
[0002] With the development of science and technology, multi-arm collaborative robots are gradually applied to people's lives, and are used in service scenarios, handling scenarios or nursing scenarios. At this time, the nursing robot is a type of multi-arm collaborative robot. The nursing robot is used in nursing scenarios and uses multiple robotic arms to perform nursing operations. In the existing technology, multiple robotic arms perform multiple action operations in the same space, and the multiple robotic arms do not have a clear active and slave relationship, resulting in poor coordination in the operation of the multiple robotic arms. They can only operate according to a single instruction and are not compatible with operations in different environments. Summary of the invention
[0003] The present application provides a dual-arm control method and system for a nursing robot, which can improve the operating accuracy of the nursing robot and ensure the safety of nursing operations.
[0004] On the one hand, the present application provides a dual-arm control method of a nursing robot, which is applied to the nursing robot, and the method comprises:
[0005] Collect the movement trajectories of the two arms of the nursing robot, and define the active arm and the slave arm for the two arms of the nursing robot;
[0006] When the active arm moves along the moving trajectory, dynamic control is triggered for the active arm and the corresponding multiple joint parts, and the dynamic control of the active arm is performed based on the fuzzy PID control algorithm;
[0007] Associating the slave arm with the master arm, defining the slave posture of the slave arm based on the current postures of the slave arm and the master arm, and establishing a posture coordination relationship between the slave posture of the slave arm and the current posture of the master arm;
[0008] When the slave arm and the active arm provide care for the patient, the contact parameters and the spatial position of the contact of the slave arm and the active arm with the patient are collected; the care paths of the slave arm and the active arm are defined according to the contact parameters, the spatial position of the contact and the movement trajectory of the slave arm and the active arm with the patient;
[0009] Define the mechanical system of the slave arm and the active arm in the nursing path, in which the nursing control logic of the slave arm and the active arm on the patient is controlled.
[0010] On the other hand, the present application provides a dual-arm control system of a nursing robot, the dual-arm control system of the nursing robot comprising:
[0011] A collection module, used for collecting the movement trajectory of the two arms of the nursing robot, and defining the active arm and the slave arm for the two arms of the nursing robot;
[0012] A dynamic control module, used for triggering dynamic control on the active arm and the corresponding multiple joint parts when the active arm moves along the moving trajectory, and performing dynamic control of the active arm based on a fuzzy PID control algorithm;
[0013] A dynamic coordination relationship module is used to associate the slave arm with the active arm, define the slave posture of the slave arm based on the current postures of the slave arm and the active arm, and construct a posture coordination relationship between the slave posture of the slave arm and the current posture of the active arm;
[0014] The nursing module is used to collect the contact parameters and spatial positions of the slave arm and the active arm on the patient when the slave arm and the active arm are nursing the patient; and define the nursing paths of the slave arm and the active arm according to the contact parameters, spatial positions and movement trajectories of the slave arm and the active arm on the patient;
[0015] The control module is used to define the mechanical system of the slave arm and the active arm in the nursing path, and in this mechanical system, control the nursing control logic of the slave arm and the active arm for the patient.
[0016] From the technical solution provided in the present application, it can be seen that the moving trajectory of the two arms of the nursing robot is collected, and the active arm and the slave arm are defined for the two arms of the nursing robot; when the active arm moves along the moving trajectory, dynamic control is triggered for the active arm and the corresponding multiple joint parts, and the dynamic control of the active arm is performed based on the fuzzy PID control algorithm; at this time, the active arm and the slave arm are introduced, and the position and force control are integrated for the active arm and the slave arm, the coordination of the active arm and the slave arm is enhanced, and the fuzzy PID control algorithm is used to control the operation of the active arm and the slave arm in different environments, and at the same time, the nursing path of the slave arm and the active arm is defined according to the contact parameters of the slave arm and the active arm with the patient, the spatial position of the contact and the moving trajectory; the mechanical system of the slave arm and the active arm in the nursing path is defined, and in this mechanical system, the nursing control logic of the slave arm and the active arm for the patient is controlled. BRIEF DESCRIPTION OF THE DRAWINGS
[0017] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.
[0018] Figure 1 is a flow chart of a dual-arm control method of a nursing robot in an embodiment of the present application;
[0019] Figure 2 is a flow chart of S11 in the dual-arm control method of the nursing robot in the embodiment of the present application;
[0020] Figure 3 is a flow chart of S12 in the dual-arm control method of the nursing robot in the embodiment of the present application;
[0021] Figure 4 is a flow chart of S13 in the dual-arm control method of the nursing robot in the embodiment of the present application;
[0022] Figure 5 is a flow chart of S14 in the dual-arm control method of the nursing robot in the embodiment of the present application;
[0023] Figure 6 is a flow chart of S15 in the dual-arm control method of the nursing robot in the embodiment of the present application;
[0024] Figure 7 is a schematic diagram of joints of a nursing robot in a dual-arm control method of a nursing robot in an embodiment of the present application;
[0025] Figure 8 Schematic diagram of the structure of the dual-arm control system of the nursing robot in the embodiment of the present application;
[0026] Fig. 9 The figure is a hardware diagram of an electronic device according to an exemplary embodiment. DETAILED DESCRIPTION
[0027] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.
[0028] See also Figures 1 to 9 , a dual-arm control method of a nursing robot, applied to the nursing robot; the dual-arm control method of the nursing robot comprises:
[0029] Step S11: collecting the movement trajectories of the arms of the nursing robot, and defining the active arm and the slave arm for the arms of the nursing robot;
[0030] Step S12: When the active arm moves along the moving trajectory, dynamic control is triggered for the active arm and the corresponding multiple joint parts, and dynamic control of the active arm is performed based on a fuzzy PID control algorithm;
[0031] Step S13: Associating the slave arm with the master arm, defining the slave posture of the slave arm based on the current postures of the slave arm and the master arm, and establishing a posture coordination relationship between the slave posture of the slave arm and the current posture of the master arm;
[0032] Step S14: when the slave arm and the active arm are caring for the patient, the contact parameters and the spatial position of the contact of the slave arm and the active arm with the patient are collected; and the nursing path of the slave arm and the active arm is defined according to the contact parameters, the spatial position of the contact and the movement trajectory of the slave arm and the active arm with the patient;
[0033] Step S15: define the mechanical system of the slave arm and the active arm in the nursing path, in which the nursing control logic of the slave arm and the active arm on the patient is controlled.
[0034] In an embodiment of the present application, through the method in the embodiment of the present application, the moving trajectory of the two arms of the nursing robot is collected, and the active arm and the slave arm are defined for the two arms of the nursing robot; when the active arm moves along the moving trajectory, dynamic control is triggered for the active arm and the corresponding multiple joint parts, and the dynamic control of the active arm is performed based on the fuzzy PID control algorithm; at this time, the active arm and the slave arm are introduced, and the position and force control are integrated for the active arm and the slave arm, the coordination of the active arm and the slave arm is enhanced, and the fuzzy PID control algorithm is used to control the operation of the active arm and the slave arm in different environments, and at the same time, the nursing path of the slave arm and the active arm is defined according to the contact parameters of the slave arm and the active arm with the patient, the spatial position of the contact and the moving trajectory; the mechanical system of the slave arm and the active arm in the nursing path is defined, and in this mechanical system, the nursing control logic of the slave arm and the active arm for the patient is controlled.
[0035] At the same time, through nursing control logic design and training learning, the coordinated movement of the two arms of the dual-arm robot is realized. The service felt by patients and auxiliary caregivers is highly similar to that of humans, which helps patients' acceptance and improves the level of robot care.
[0036] In step S11, the movement trajectories of the arms of the nursing robot are collected, and the active arm and the slave arm are defined for the arms of the nursing robot;
[0037] In the specific implementation process of this application, the specific steps may be:
[0038] S111: locating the patient's position, and locating the target position according to the patient's position;
[0039] S112: collecting the current positions of the arms of the nursing robot, and determining the moving trajectories of the arms of the nursing robot according to the current positions of the arms of the nursing robot and the target positions;
[0040] S113: Collecting the movement trajectories of the arms of the nursing robot;
[0041] S114: Define the active arm and the slave arm based on the moving trajectory and the nursing instructions of the nursing robot. At this time, the active arm and the slave arm are controlled by the same controller, and the coordinated control between the active arm and the slave arm is triggered.
[0042] In an embodiment of the present application, the patient's position is located, and the target position is located based on the patient's position. At the same time, the current position of the arms of the nursing robot is collected, and the current position and target position of the arms of the nursing robot are introduced. Therefore, the moving trajectory of the arms of the nursing robot is determined based on the current position and target position of the arms of the nursing robot, so as to clarify the moving trajectory of the arms of the nursing robot.
[0043] At this time, the movement trajectory of the two arms of the nursing robot is collected; the active arm and the slave arm are defined based on the movement trajectory and the nursing instructions of the nursing robot. At this time, the active arm and the slave arm are controlled by the same controller, and the collaborative control between the active arm and the slave arm is triggered.
[0044] In order to facilitate the division of the two arms of the nursing robot into the active arm and the slave arm, and clarify the active relationship of the active arm and the slave relationship of the slave arm, so as to facilitate the control of the active relationship of the active arm and the slave relationship of the slave arm, and further construct the slave relationship of the slave arm to the active arm, therefore, based on the use of the same controller for control of the active arm and the slave arm, and triggering the coordinated control between the active arm and the slave arm, the coordinated action of the active arm and the slave arm is guaranteed.
[0045] At the same time, by studying the positional relationship between the two robot arms and training the nursing robot by sampling the positional relationship of human arms, the relative positional relationship between the active arm and the slave arm of the nursing robot at each moment is similar to that of human arms, and the simulation design and training of human arms are realized. Optionally, since the kinematic parameter system of the active arm and the slave arm needs to be established, the kinematic parameters of the active arm and the slave arm need to be checked at all times, and the active arm and the slave arm need to be well calibrated. The patient usually lies on the bed, and the position of the nursing robot has a certain flexibility. Through the robot vision and the reminder of the auxiliary caregiver, the arms are stretched out to the force position, such as the back of the patient's neck, knee bend, etc. The coordination of the arms is extremely important. The algorithm and theory of the coordinated control of the position of the arms are the basis of the force control of the dual-arm robot.
[0046] In step S12, when the active arm moves along the moving trajectory, dynamic control is triggered for the active arm and the corresponding multiple joint parts, and the dynamic control of the active arm is performed based on the fuzzy PID control algorithm;
[0047] In the specific implementation process of this application, the specific steps may be:
[0048] S121: When the active arm moves along the moving trajectory, dynamic parameters of the active arm during the movement process are collected;
[0049] S122: Associating the dynamic parameters of the active arm during the movement and the joint parameters of each joint;
[0050] S123: constructing a dynamic control relationship according to the dynamic parameters of the active arm and the joint parameters of each joint;
[0051] S124: triggering dynamic control between the active arm and the corresponding multiple joint parts based on the dynamic control relationship to ensure the dynamic stability of the active arm during the movement;
[0052] S125: collecting the fuzzy PID control algorithm and associating the fuzzy PID control algorithm to the active arm;
[0053] S126: Perform dynamic control of the active arm based on the fuzzy PID control algorithm.
[0054] In an embodiment of the present application, when the active arm moves along the moving trajectory, the dynamic parameters of the active arm during the movement are collected, and control is performed based on the dynamic parameters of the active arm during the movement. At this time, the dynamic parameters of the active arm during the movement and the joint parameters of each joint are associated, and a dynamic control relationship is constructed based on the dynamic parameters of the active arm and the joint parameters of each joint, so that multiple joints are managed and controlled according to the dynamic control relationship, and the action of the active arm is constructed based on the movement of multiple joints, thereby ensuring the consistency of the actions of multiple joints on the active arm and performing dynamic control on multiple joints.
[0055] Optionally, the configuration of the nursing robot is based on the configuration of the human arm. On the premise that the number of degrees of freedom meets the working requirements, when the number of degrees of freedom is larger, there are redundant degrees of freedom, and the flexibility of the robot arm is good; conversely, when the number of degrees of freedom is smaller, the rigidity of the robot arm is good and the control is simpler. After comprehensive consideration, the dual-arm collaborative medical robot arm is designed with 6 degrees of freedom, which can meet the nursing needs such as holding operations.
[0056] At this time, the dynamic control between the active arm and the corresponding multiple joint parts is triggered based on the dynamic control relationship to ensure the dynamic stability of the active arm during the movement; the fuzzy PID control algorithm is collected and associated with the active arm; the dynamic control of the active arm is performed based on the fuzzy PID control algorithm, thereby introducing the fuzzy PID control algorithm, and interacting with the fuzzy PID control algorithm and the active arm, so that the active arm can move stably under the control of the fuzzy PID control algorithm to cope with different working environments.
[0057] Furthermore, the position control of the active arm and the driven arm traditionally adopts PID control, which has the advantages of simple structure, strong robustness, good stability, etc. The principle is to use the difference between the expected value and the actual output value as the control error, and use the linear combination of its proportion, integral and differential with the controlled object to form a closed-loop negative feedback control system. Compared with the traditional PID algorithm, this method adopts the fuzzy PID control algorithm. The control error value is input into the fuzzy controller, and the fuzzy controller outputs the real-time PID parameters to the fuzzy PID controller, which can realize online custom PID parameters, making up for the deficiency that the traditional PID cannot cope with different working environments.
[0058] Specifically, based on the fuzzy PID control algorithm, the control error value is input into the fuzzy PID controller, and the fuzzy controller outputs the real-time PID parameters to the fuzzy PID controller, which can realize online customization of PID parameters to cope with different working environments. At the same time, the active arm and the slave arm are introduced, and the position and force control are integrated for the active arm and the slave arm, which enhances the coordination of the active arm and the slave arm, and the fuzzy PID control algorithm is used to control the operation of the active arm and the slave arm in different environments.
[0059] Therefore, the corresponding fuzzy PID controller is matched based on the fuzzy PID control algorithm. At the same time, the fuzzy PID control algorithm inputs the control error value into the fuzzy PID controller, and the fuzzy controller outputs the real-time PID parameters to the fuzzy PID controller, which can realize online custom PID parameters to cope with different working environments;
[0060] The fuzzy PID controller maps the adjustment factors of the proportional term P, the integral term I, and the differential term D through the fuzzy logic rule table according to the current error and the error change rate; if the fuzzy set is defined as {NB, NS, ZO, PS, PB} (representing negative large, negative small, zero, positive small, and positive large, respectively), the calculation of the adjustment factor can be expressed as:
[0061]
[0062] Among them, u P 、u I 、u D Represent the adjusted proportional, integral, and differential coefficients, respectively, w i is the weight of the i-th fuzzy rule, μ Pi , μ Ii , μ Di is the membership function value, n is the number of fuzzy rules, e, ec, ei are the current error, error change rate and error integral respectively;
[0063] In addition, in multiple joints, the motor drives the arm to rotate through the reducer, and the motor independently controls the rotation of the corresponding joint to achieve voltage balance, torque balance and coupling of motor voltage and current. The simultaneous equations are as follows:
[0064]
[0065] Among them, e a is the armature voltage (V), R a is the armature resistance (Ω), i a is the armature current (A), L a is the armature inductance (H), e m is the armature back electromotive force (V), J is the equivalent inertia moment converted to the motor shaft (N·m), B is the equivalent viscous friction coefficient converted to the motor shaft, θ m is the motor shaft rotation angle (rad), T is the motor output torque (N·m), K T is the torque constant (N·m / A), K e is the back-electromotive force constant (V / (rad / s)); the Laplace transform is performed on the simultaneous equations to obtain the transfer function of the rotary joint of the robot arm.
[0066] At the same time, the fuzzy controller tool is called in the simulation tool, and the input is set to the ideal joint angle change curve. The actual and ideal joint angle errors and their change rates are used as the input of the fuzzy controller. The fuzzy controller runs and outputs three change increments of the PID parameters to calculate the output of the control system joint rotation angle. Through joint simulation, the ideal rotation angles of the joints of the two arms obtained by trajectory planning are input into the fuzzy PID controller, and the trajectory position of the two arms is simulated and calculated over time to control the coordinated movement of the robot's two arms. The planned movement trajectory basically coincides with the ideal curve to avoid collisions between the two arms and inconsistent lifting angles of the two arms.
[0067] In step S13, the slave arm and the master arm are associated, the slave posture of the slave arm is defined based on the current postures of the slave arm and the master arm, and a posture coordination relationship is established between the slave posture of the slave arm and the current posture of the master arm;
[0068] In the specific implementation process of this application, the specific steps may be:
[0069] S131: Associating the slave arm with the master arm;
[0070] S132: collecting the current postures of the slave arm and the master arm;
[0071] S133: defining a slave posture of the slave arm based on the current postures of the slave arm and the master arm;
[0072] S134: Freeze the slave posture of the slave arm and the current posture of the master arm;
[0073] S135: Dynamically interact based on the slave posture of the slave arm and the current posture of the master arm, and establish a posture coordination relationship between the slave posture of the slave arm and the current posture of the master arm.
[0074] In an embodiment of the present application, the slave arm and the master arm are associated, and the current postures of the slave arm and the master arm are controlled. At this time, the current postures of the slave arm and the master arm are collected; the slave posture of the slave arm is defined based on the current postures of the slave arm and the master arm, thereby clarifying the slave posture of the slave arm, and the slave posture of the slave arm is regulated as the current posture of the master arm is regulated, thereby ensuring the association of the slave arm with the master arm.
[0075] At this time, the slave posture of the slave arm and the current posture of the active arm are frozen; dynamic interaction is performed based on the slave posture of the slave arm and the current posture of the active arm, and a posture coordination relationship is constructed between the slave posture of the slave arm and the current posture of the active arm, thereby introducing a posture coordination relationship for the slave arm and the active arm, ensuring the relative stability of the posture of the slave arm and the posture of the active arm, thereby achieving the operational stability of the dual arms of the nursing robot.
[0076] Optionally, in terms of control mechanism, one arm of the nursing robot is regarded as the active arm, and the other is regarded as the slave arm. In terms of control algorithm, the active arm moves according to a predetermined trajectory, and the slave arm applies a certain force on the operating object according to feedback data, and then follows the movement of the object. In the traditional algorithm, the active arm only adopts pure position control, while the slave arm only adopts pure driving force control. In this application, position and force control are introduced for both the active arm and the slave arm. Since both the active arm and the slave arm have position control, the coordinated movement of the two arms of the nursing robot can be ensured; and force feedback can ensure that some constraints of the nursing robot in the coordinated movement of the two arms are satisfied. In addition, the active arm and the slave arm use the same controller, which solves the problem of control command coupling between the active arm and the slave arm.
[0077] Therefore, the coordinated operation object of the nursing robot's two arms is the human body, and the human body's contour features are small deformation and easy positioning, which can be regarded as a soft object with a known posture (through visual recognition). In the master-slave collaborative control algorithm, the two robotic arms of the nursing robot are divided into the master arm (MasterArm) and the slave arm (SlaveArm). In the actual algorithm, the motion trajectory of the master robotic arm is first determined according to the trajectory of the target point, and then the motion planning of the slave robotic arm is carried out to achieve efficient, accurate, stable and reliable coordinated operation of the robotic arms based on the optimal solution.
[0078] Since the motion constraint relationship between the two arms changes at a specific time node, it is necessary to consider separating the coupled motion trajectories of the two arms to obtain the corresponding robotic arm states, that is, to establish the kinematic constraint relationship of the two-arm collaboration. In order to ensure that the lower limbs and back neck of the patient can reach the designated position safely and effectively during the nursing robot's lifting operation, a tightly coordinated motion planning strategy is adopted. In terms of algorithm implementation, the path points of the end effectors of the two arms are planned in Cartesian space, which ensures the stability and accuracy of the active and driven arms and realizes coordinated lifting of the two arms.
[0079] In addition, in the posture coordination relationship, the adaptive strategy based on the radial basis function neural network controls the slave posture of the slave arm and the current posture of the master arm; at this time, the radial basis function neural network updates the weight vector through the gradient descent method to minimize the contact force error of the system; the gradient descent method adjusts the weight vector according to the error signal and the learning rate. The formula of the weight vector is as follows:
[0080] w i (k+1)=w i (k)+μe(k)h i (x(k))
[0081] Among them, μ is the learning rate, e(k) is the error signal, k is the discrete time step, and the radial basis function neural network is calculated as follows:
[0082]
[0083] The output vector y is a function of the mass and damping parameters of the admittance controller, w i is the weight vector, h i (x) is the Gaussian basis function;
[0084] In radial basis function neural networks, weight updates involve the gradient descent method, and the specific formula is:
[0085]
[0086] Among them, Δwj is the update amount of the jth weight, η is the learning rate, E is the total error of the network, is the partial derivative of the error with respect to the weight.
[0087] At this time, this method adopts an adaptive strategy based on radial basis function neural network, based on the best approximation characteristics of radial basis function neural network, as well as the characteristics of relatively simple structure and fast calculation speed, to improve the situation of uncoordinated movement of robot arms affected by the environment in actual environment. This method has the advantages of stronger environmental adaptability, system stability, tracking accuracy and response speed.
[0088] The radial basis function neural network is a neural network based on radial basis function, which consists of three layers: input layer, hidden layer and output layer. The neural network structure adopted in this method is a 2-5-2 network structure; the input layer receives the system state and error signal, the hidden layer uses the Gaussian basis function as the activation function, and the output layer uses a linear function to output the parameters of the admittance controller.
[0089] Therefore, the introduction of a radial basis function neural network can significantly improve the accuracy and dynamic response speed of force tracking, and converge to the desired force faster when disturbed by the external environment, reduce the contact force peak, and improve the robustness of the system and the safety of nursing. Using an adaptive strategy based on a radial basis function neural network, the dual-arm motion is modeled and learned, which improves the environmental adaptability, system stability, tracking accuracy and response speed of the system. Especially when dealing with the flexible state of the nursing object (human body), the accuracy and dynamic response speed of force tracking are significantly enhanced, the impact of external interference is reduced, and the safety of nursing operations is ensured.
[0090] S14: when the slave arm and the active arm are caring for the patient, collecting contact parameters and spatial positions of the slave arm and the active arm on the patient; defining a care path of the slave arm and the active arm according to the contact parameters, spatial positions and movement trajectories of the slave arm and the active arm on the patient;
[0091] In the specific implementation process of this application, the specific steps may be:
[0092] S141: when the slave arm and the active arm are caring for the patient, collecting contact parameters and spatial positions of the slave arm and the active arm on the patient;
[0093] S142: Associating the contact parameters, spatial positions of contact, and movement trajectories of the slave arm and the master arm on the patient;
[0094] S143: defining a nursing path of the slave arm and the active arm according to the contact parameters of the slave arm and the active arm with the patient, the spatial position of the contact, and the movement trajectory;
[0095] In an embodiment of the present application, when the slave arm and the active arm are caring for the patient, the contact parameters and spatial position of the slave arm and the active arm with the patient are collected, and the contact parameters and spatial position of the slave arm and the active arm with the patient are introduced, so as to associate the contact parameters, spatial position of the contact and the movement trajectory of the slave arm and the active arm with the patient, and then the nursing path of the slave arm and the active arm is defined according to the contact parameters, spatial position of the contact and the movement trajectory of the slave arm and the active arm with the patient, so as to clarify the nursing path, and control the slave arm and the active arm in performing nursing actions on the patient through the nursing path, thereby ensuring the stability of the slave arm and the active arm in performing nursing actions on the patient.
[0096] S15: define the mechanical system of the slave arm and the active arm in the nursing path, in which the nursing control logic of the slave arm and the active arm for the patient is controlled;
[0097] In the specific implementation process of this application, the specific steps may be:
[0098] S151: Associating the slave arm, the master arm and the nursing path, and defining the mechanical system of the slave arm and the master arm in the nursing path;
[0099] S152: In this mechanical system, the nursing control logic of the slave arm and the active arm for the patient is controlled. At this time, the dynamic relationship between the slave arm, the active arm and the patient is constructed based on the nursing control logic, and the coordination relationship between the slave arm and the active arm is regulated based on the patient's posture.
[0100] In an embodiment of the present application, the slave arm, the master arm and the care path are associated, and the mechanical system of the slave arm and the master arm in the care path is defined, so as to control the mechanical states of the slave arm and the master arm during the movement, and perform dynamic adjustments based on the mechanical system.
[0101] Therefore, in this mechanical system, the nursing control logic of the slave arm and the active arm for the patient is controlled. At this time, the dynamic relationship between the slave arm, the active arm and the patient is constructed based on the nursing control logic, and the coordination relationship between the slave arm and the active arm is regulated based on the patient's posture.
[0102] In an embodiment of the present application, through the method in the embodiment of the present application, the moving trajectory of the two arms of the nursing robot is collected, and the active arm and the slave arm are defined for the two arms of the nursing robot; when the active arm moves along the moving trajectory, dynamic control is triggered for the active arm and the corresponding multiple joint parts, and the dynamic control of the active arm is performed based on the fuzzy PID control algorithm; at this time, the active arm and the slave arm are introduced, and the position and force control are integrated for the active arm and the slave arm, the coordination of the active arm and the slave arm is enhanced, and the fuzzy PID control algorithm is used to control the operation of the active arm and the slave arm in different environments, and at the same time, the nursing path of the slave arm and the active arm is defined according to the contact parameters of the slave arm and the active arm with the patient, the spatial position of the contact and the moving trajectory; the mechanical system of the slave arm and the active arm in the nursing path is defined, and in this mechanical system, the nursing control logic of the slave arm and the active arm for the patient is controlled.
[0103] System Example
[0104] See also Figure 8 , Figure 8 It is a schematic diagram of the structural composition of the dual-arm control system of the nursing robot in the embodiment of the present application.
[0105] like Figure 8 As shown, a dual-arm control system of a nursing robot, the dual-arm control system of the nursing robot comprises:
[0106] The acquisition module 21 is used to acquire the movement trajectory of the arms of the nursing robot and define the active arm and the slave arm for the arms of the nursing robot;
[0107] A dynamic control module 22, for triggering dynamic control on the active arm and the corresponding multiple joint parts when the active arm moves along the moving trajectory, and performing dynamic control of the active arm based on a fuzzy PID control algorithm;
[0108] A dynamic coordination relationship module 23 is used to associate the slave arm with the master arm, define the slave posture of the slave arm based on the current postures of the slave arm and the master arm, and construct a posture coordination relationship between the slave posture of the slave arm and the current posture of the master arm;
[0109] The nursing module 24 is used to collect the contact parameters and the spatial position of the slave arm and the active arm on the patient when the slave arm and the active arm are nursing the patient; and define the nursing path of the slave arm and the active arm according to the contact parameters, the spatial position of the contact and the movement trajectory of the slave arm and the active arm on the patient;
[0110] The control module 25 is used to define the mechanical system of the slave arm and the active arm in the nursing path, and in the mechanical system, control the nursing control logic of the slave arm and the active arm on the patient.
[0111] Device Embodiment
[0112] See also Fig. 9 , refer to the following Fig. 9 The electronic device 40 according to this embodiment of the present application is described. Fig. 9 The electronic device 40 shown is merely an example and should not bring any limitation to the functions and scope of use of the embodiments of the present application.
[0113] like Fig. 9 As shown, the electronic device 40 is in the form of a general computing device. The components of the electronic device 40 may include but are not limited to: at least one processing unit 41, at least one storage unit 42, and a bus 43 connecting different system components (including the storage unit 42 and the processing unit 41).
[0114] The storage unit stores program codes, which can be executed by the processing unit 41, so that the processing unit 41 executes the steps described in the above “Embodiment Method” section of this specification according to various exemplary implementations of the present application.
[0115] The storage unit 42 may include a readable medium in the form of a volatile storage unit, such as a random access memory unit (RAM) 421 and / or a cache memory unit 422 , and may further include a read-only memory unit (ROM) 423 .
[0116] The storage unit 42 may also include a program / utility 424 having a set (at least one) of program modules 425, such program modules 425 including but not limited to: an operating system, one or more application programs, other program modules, and program data, each of which or some combination may include an implementation of a network environment.
[0117] Bus 43 may represent one or more of several types of bus structures, including a memory unit bus or memory unit controller, a peripheral bus, an accelerated graphics port, a processing unit, or a local bus using any of a variety of bus architectures.
[0118] The electronic device 40 may also communicate with one or more external devices (e.g., keyboards, pointing devices, Bluetooth devices, etc.), one or more devices that enable a user to interact with the electronic device 40, and / or any device that enables the electronic device 40 to communicate with one or more other computing devices (e.g., routers, modems, etc.). Such communication may be performed via an input / output (I / O) interface 44. Furthermore, the electronic device 40 may also communicate with one or more networks (e.g., a local area network (LAN), a wide area network (WAN), and / or a public network, such as the Internet) via a network adapter 45. Fig. 9 As shown, the network adapter 45 communicates with other modules of the electronic device 40 via the bus 43. It should be understood that although Fig. 9 Not shown, other hardware and / or software modules may be used in conjunction with the electronic device 40, including but not limited to: microcode, device drivers, redundant processing units, external disk drive arrays, RAID systems, tape drives, and data backup planning systems.
[0119] Through the description of the above implementation, it is easy for those skilled in the art to understand that the example implementation described here can be implemented by software, or by software combined with necessary hardware. Therefore, the technical solution according to the implementation of the present disclosure can be embodied in the form of a software product, which can be stored in a non-volatile storage medium (which can be a CD-ROM, a USB flash drive, a mobile hard disk, etc.) or on a network, including several instructions to enable a computing device (which can be a personal computer, a server, a terminal device, or a network device, etc.) to execute the method according to the implementation of the present disclosure.
[0120] Those skilled in the art can understand that all or part of the steps in the various methods of the above embodiments can be completed by instructing related hardware through a program, and the program can be stored in a computer-readable storage medium, and the storage medium can include: a read-only memory (ROM), a random access memory (RAM), a disk or an optical disk, etc. In addition, it stores computer program instructions, and when the computer program instructions are executed by a computer, the computer executes the above method.
[0121] In addition, the above describes in detail the dual-arm control method and system of the nursing robot provided in the embodiments of the present application. Specific examples are used herein to illustrate the principles and implementation methods of the present application. The description of the above embodiments is only used to help understand the method of the present application and its core idea. At the same time, for general technical personnel in this field, according to the ideas of the present application, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as a limitation on the present application.
Claims
1. A dual-arm control method for a nursing robot, It is characterized in that The method is applied to a nursing robot, and the method comprises: The dual-arm control method of the nursing robot comprises: Collect the movement trajectories of the two arms of the nursing robot, and define the active arm and the slave arm for the two arms of the nursing robot; When the active arm moves along the moving trajectory, dynamic control is triggered for the active arm and the corresponding multiple joint parts, and the dynamic control of the active arm is performed based on the fuzzy PID control algorithm; Associating the slave arm with the master arm, defining the slave posture of the slave arm based on the current postures of the slave arm and the master arm, and establishing a posture coordination relationship between the slave posture of the slave arm and the current posture of the master arm; When the slave arm and the active arm provide care for the patient, the contact parameters and the spatial position of the contact of the slave arm and the active arm with the patient are collected; the care paths of the slave arm and the active arm are defined according to the contact parameters, the spatial position of the contact and the movement trajectory of the slave arm and the active arm with the patient; Define the mechanical system of the slave arm and the active arm in the nursing path, in which the nursing control logic of the slave arm and the active arm on the patient is controlled.
2. The dual-arm control method of the nursing robot according to claim 1, It is characterized in that The collecting of the moving trajectories of the two arms of the nursing robot and defining the active arm and the driven arm for the two arms of the nursing robot include: Locate the patient's location, and locate the target location based on the patient's location; Collecting the current position of the arms of the nursing robot, and determining the moving trajectory of the arms of the nursing robot according to the current position of the arms of the nursing robot and the target position; Collect the movement trajectory of the arms of the nursing robot; The active arm and the slave arm are defined based on the moving trajectory and the nursing instructions of the nursing robot. At this time, the active arm and the slave arm are controlled by the same controller, and the coordinated control between the active arm and the slave arm is triggered.
3. The dual-arm control method of the nursing robot according to claim 2, It is characterized in that When the active arm moves along the moving trajectory, dynamic control is triggered for the active arm and the corresponding multiple joint parts, and dynamic control of the active arm is performed based on a fuzzy PID control algorithm, including: When the active arm moves along the moving trajectory, dynamic parameters of the active arm during the moving process are collected; Associate the dynamic parameters of the active arm during movement and the joint parameters of each joint; A dynamic control relationship is established according to the dynamic parameters of the active arm and the joint parameters of each joint; Based on the dynamic control relationship, the dynamic control between the active arm and the corresponding multiple joint parts is triggered to ensure the dynamic stability of the active arm during the movement; Collect the fuzzy PID control algorithm and associate the fuzzy PID control algorithm with the active arm; The dynamic control of the active arm is carried out based on the fuzzy PID control algorithm.
4. The dual-arm control method of the nursing robot according to claim 3, It is characterized in that When the active arm moves along the moving trajectory, dynamic control is triggered for the active arm and the corresponding multiple joint parts, and the dynamic control of the active arm is performed based on the fuzzy PID control algorithm, and further includes: Based on the fuzzy PID control algorithm, the control error value is input into the fuzzy PID controller, and the fuzzy controller outputs the real-time PID parameters to the fuzzy PID controller, which can realize online customization of PID parameters to cope with different working environments; The corresponding fuzzy PID controller is matched based on the fuzzy PID control algorithm. At the same time, the fuzzy PID control algorithm inputs the control error value into the fuzzy PID controller, and the fuzzy controller outputs the real-time PID parameters to the fuzzy PID controller, which can realize online custom PID parameters to cope with different working environments; The fuzzy PID controller maps the adjustment factors of the proportional term P, the integral term I, and the differential term D through the fuzzy logic rule table according to the current error and the error change rate; if the fuzzy set is defined as {NB, NS, ZO, PS, PB} (representing negative large, negative small, zero, positive small, and positive large, respectively), the calculation of the adjustment factor can be expressed as: Among them, uP, uI, and uD represent the adjusted proportional, integral, and differential coefficients respectively, wi is the weight of the i-th fuzzy rule, μPi, μIi, and μDi are the membership function values, n is the number of fuzzy rules, and e, ec, and ei are the current error, error change rate, and error integral respectively; In addition, in multiple joints, the motor drives the arm to rotate through the reducer, and the motor independently controls the rotation of the corresponding joint to achieve voltage balance, torque balance and coupling of motor voltage and current. The simultaneous equations are as follows: Among them, e a is the armature voltage (V), R a is the armature resistance (Ω), i a is the armature current (A), L a is the armature inductance (H), e m is the armature back electromotive force (V), J is the equivalent inertia moment converted to the motor shaft (N·m), B is the equivalent viscous friction coefficient converted to the motor shaft, θ m is the motor shaft rotation angle (rad), T is the motor output torque (N·m), K T is the torque constant (N·m / A), K e is the back-electromotive force constant (V / (rad / s)); the Laplace transform is performed on the simultaneous equations to obtain the transfer function of the rotary joint of the robot arm.
5. The dual-arm control method of the nursing robot according to claim 1, It is characterized in that The associating the slave arm with the master arm, defining the slave posture of the slave arm based on the current postures of the slave arm and the master arm, and establishing a posture coordination relationship between the slave posture of the slave arm and the current posture of the master arm, includes: Associating the slave arm with the master arm; Collect the current posture of the slave arm and the master arm; Defining a slave posture of the slave arm based on the current postures of the slave arm and the master arm; Freeze the slave posture of the slave arm and the current posture of the master arm; Dynamic interaction is performed based on the slave posture of the slave arm and the current posture of the active arm, and a posture coordination relationship is established between the slave posture of the slave arm and the current posture of the active arm.
6. The dual-arm control method of the nursing robot according to claim 5, It is characterized in that The associating the slave arm with the master arm, defining the slave posture of the slave arm based on the current postures of the slave arm and the master arm, and establishing a posture coordination relationship between the slave posture of the slave arm and the current posture of the master arm, further includes: In the posture coordination relationship, the adaptive strategy based on the radial basis function neural network controls the slave posture of the slave arm and the current posture of the master arm; at this time, the radial basis function neural network updates the weight vector through the gradient descent method to minimize the contact force error of the system; the gradient descent method adjusts the weight vector according to the error signal and the learning rate. The formula of the weight vector is as follows: wi(k+1)=wi(k)+ue(k)h,(x(k)) Among them, μ is the learning rate, e(k) is the error signal, k is the discrete time step, and the radial basis function neural network is calculated as follows: The output vector y is a function of the mass and damping parameters of the admittance controller, w i is the weight vector, h i (x) is the Gaussian basis function; In radial basis function neural networks, weight updates involve the gradient descent method, and the specific formula is: Among them, Δwj is the update amount of the jth weight, η is the learning rate, E is the total error of the network, is the partial derivative of the error with respect to the weight.
7. The dual-arm control method of the nursing robot according to claim 6, It is characterized in that When the slave arm and the active arm are caring for the patient, the contact parameters and the spatial position of the contact between the slave arm and the active arm and the patient are collected; The nursing paths of the slave arm and the active arm are defined according to the contact parameters of the slave arm and the active arm on the patient, the spatial position of the contact, and the movement trajectory, including: When the slave arm and the active arm provide care for the patient, the contact parameters and the contact spatial positions of the slave arm and the active arm on the patient are collected; The contact parameters, spatial location of the contact, and movement trajectory of the slave arm and the master arm on the patient are associated.
8. The dual-arm control method of the nursing robot according to claim 7, It is characterized in that When the slave arm and the active arm are caring for the patient, the contact parameters and the spatial position of the contact between the slave arm and the active arm and the patient are collected; The nursing path of the slave arm and the active arm is defined according to the contact parameters of the slave arm and the active arm to the patient, the spatial position of the contact, and the movement trajectory, and also includes: The nursing paths of the slave arm and the active arm are defined according to the contact parameters of the slave arm and the active arm with the patient, the spatial position of the contact, and the movement trajectory.
9. The dual-arm control method of the nursing robot according to claim 7, It is characterized in that The mechanical system defining the slave arm and the active arm in the nursing path, in which the nursing control logic of the slave arm and the active arm for the patient is controlled, includes: Associating the slave arm, the master arm and the nursing path, and defining the mechanical system of the slave arm and the master arm in the nursing path; In this mechanical system, the nursing control logic of the slave arm and the active arm for the patient is controlled. At this time, the dynamic relationship between the slave arm, the active arm and the patient is constructed based on the nursing control logic, and the coordination relationship between the slave arm and the active arm is regulated based on the patient's posture.
10. A dual-arm control system for a nursing robot. It is characterized in that The dual-arm control system of the nursing robot is applied to the dual-arm control method of the nursing robot according to any one of claims 1 to 9, and the dual-arm control system of the nursing robot includes: A collection module, used for collecting the movement trajectory of the two arms of the nursing robot, and defining the active arm and the slave arm for the two arms of the nursing robot; A dynamic control module, used for triggering dynamic control on the active arm and the corresponding multiple joint parts when the active arm moves along the moving trajectory, and performing dynamic control of the active arm based on a fuzzy PID control algorithm; A dynamic coordination relationship module is used to associate the slave arm with the active arm, define the slave posture of the slave arm based on the current postures of the slave arm and the active arm, and construct a posture coordination relationship between the slave posture of the slave arm and the current posture of the active arm; The nursing module is used to collect the contact parameters and spatial positions of the slave arm and the active arm on the patient when the slave arm and the active arm are nursing the patient; and define the nursing paths of the slave arm and the active arm according to the contact parameters, spatial positions and movement trajectories of the slave arm and the active arm on the patient; The control module is used to define the mechanical system of the slave arm and the active arm in the nursing path, and in this mechanical system, control the nursing control logic of the slave arm and the active arm for the patient.
