WO2024066063A1 - 协作机械臂及其运动控制方法、碰撞检测方法、控制系统 - Google Patents

协作机械臂及其运动控制方法、碰撞检测方法、控制系统 Download PDF

Info

Publication number
WO2024066063A1
WO2024066063A1 PCT/CN2022/138465 CN2022138465W WO2024066063A1 WO 2024066063 A1 WO2024066063 A1 WO 2024066063A1 CN 2022138465 W CN2022138465 W CN 2022138465W WO 2024066063 A1 WO2024066063 A1 WO 2024066063A1
Authority
WO
WIPO (PCT)
Prior art keywords
joint
theoretical
servo motor
motion state
torque
Prior art date
Application number
PCT/CN2022/138465
Other languages
English (en)
French (fr)
Inventor
黄睿
郎需林
姜宇
邓少敏
刘毅
林壮
Original Assignee
深圳市越疆科技有限公司
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 深圳市越疆科技有限公司 filed Critical 深圳市越疆科技有限公司
Publication of WO2024066063A1 publication Critical patent/WO2024066063A1/zh

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls

Definitions

  • the present application relates to the field of robot control technology, and in particular to a collaborative robot arm and its motion control method, collision detection method, control system, industrial robot and computer storage medium.
  • Collaborative robotic arms are robots designed to interact directly with humans in collaborative areas. They have advantages that traditional industries do not have, such as rapid deployment and human-machine collaborative safety. Safety is the main feature and advantage of collaborative robotic arms. When colliding with humans or the environment, collaborative robotic arms must be able to quickly identify collision signals and take strategies to avoid further collision damage.
  • a current collaborative robot arm When a current collaborative robot arm collides with an obstacle during movement, it will output a larger current to continue driving the collaborative robot arm, so that the collaborative robot arm can cross the obstacle and continue moving along a predetermined trajectory; and during the period when the current gradually increases from the current value of normal movement, the collaborative robot arm continues to apply force to the obstacle until the current reaches a larger current threshold, or even the current threshold is the rated current of the joint, or the current is limited after a collision signal is detected.
  • the collaborative robot arm will cause greater damage to the obstacle; especially when the obstacle is a person, the collaborative robot arm will cause greater harm to the person; at the same time, it will also cause considerable damage to the collaborative robot arm itself.
  • the present application provides a collaborative robot arm and its motion control method, collision detection method, control system, industrial robot and computer storage medium.
  • the main technical problem solved is how to limit the driving current and output torque output by the collaborative robot arm to reduce the damage caused by the collaborative robot arm when a collision occurs.
  • the present application provides a motion control method of a collaborative robot arm, comprising:
  • the driving current output to the joint servo motor is controlled not to exceed the current limiting range.
  • the present application provides another motion control method of a collaborative robot arm, which is applied to a servo controller and includes:
  • the theoretical torque is used by the servo driver to calculate the current limit range of the joint servo motor and to control the driving current output to the joint servo motor so as not to exceed the current limit range.
  • the present application provides another motion control method of a collaborative robot arm, which is applied to a servo driver of any joint, and includes:
  • the driving current output to the joint servo motor is controlled not to exceed the current limit range.
  • the present application provides a motion control system for a collaborative robot arm, including a host computer, a servo controller, servo drivers for multiple joints, and multiple joint servo motors;
  • the host computer is connected to the servo controller for communication, and the servo controller is connected to each of the servo drivers for communication, and each of the servo drivers is used to drive a corresponding joint servo motor;
  • the motion control system of the collaborative robot arm is configured with a motion control function when working, and the motion control function includes:
  • the present application provides a collision detection method for a collaborative robot arm, and the collision detection method comprises:
  • the theoretical joint motion state of the joint is calculated
  • the present application provides another collision detection method for a collaborative robot arm, wherein the collision detection method is applied to a servo controller, comprising:
  • the theoretical joint motion state of the joint is calculated
  • the theoretical torque is used by the servo driver to calculate the current limit range of the joint servo motor and to control the driving current output to the joint servo motor not to exceed the current limit range.
  • the present application provides another collision detection method for a collaborative robot arm, wherein the collision detection method is applied to a servo driver of any joint, and comprises:
  • the present application provides a control system for a collaborative robot arm, including a host computer, a servo controller, servo drivers for multiple joints, and multiple joint servo motors;
  • the host computer is connected to the servo controller for communication, and the servo controller is connected to the servo driver of each joint for communication, and the servo driver of each joint is used to drive the corresponding joint servo motor;
  • the control system of the collaborative robot arm is configured with a collision detection function when working, and the collision detection function includes:
  • the present application also provides a collaborative robot arm, which includes a processor and a memory coupled to the processor;
  • the memory is used to store program data
  • the processor is used to execute the program data to implement the above-mentioned motion control method and/or the above-mentioned collision detection method.
  • the present application also provides an industrial robot, the industrial robot comprising a processor and a memory coupled to the processor;
  • the memory is used to store program data
  • the processor is used to execute the program data to implement the above-mentioned motion control method and/or the above-mentioned collision detection method.
  • the present application also provides a computer storage medium, which is used to store program data.
  • program data When the program data is executed by a processor, it is used to implement the above motion control method and/or the above collision detection method.
  • the motion control system calculates the theoretical motion state of the joint according to the motion control task; calculates the theoretical torque of the joint servo motor according to the theoretical motion state of the joint; calculates the current limit range of the joint servo motor according to the theoretical torque; and controls the drive current output to the joint servo motor not to exceed the current limit range.
  • the above method can use the theoretical motion state of the joint and the theoretical torque of each joint servo motor to directly limit the drive current of each joint, effectively control the output torque of each joint, reduce the damage caused by the collaborative robot arm to the collision obstacle or person in the event of a collision, and effectively protect the collaborative robot arm and the person.
  • FIG1 is a schematic flow chart of a first embodiment of a motion control method provided by the present application.
  • FIG2 is a schematic diagram of the hardware structure of an embodiment of a collaborative robot arm in a motion control system provided by the present application;
  • FIG3 is a flow chart of an embodiment of a motion control method provided by the present application.
  • FIG4 is a schematic diagram of the hardware structure of an embodiment of a motion control system provided by the present application.
  • FIG5 is a schematic diagram of the hardware structure of another embodiment of the motion control system provided by the present application.
  • FIG6 is a flow chart of a second embodiment of a motion control method provided by the present application.
  • FIG7 is a schematic flow chart of a third embodiment of a motion control method provided by the present application.
  • FIG8 is a schematic diagram of a flow chart of a first embodiment of a collision detection method provided by the present application.
  • FIG9 is a flow chart of another embodiment of the motion control method provided by the present application.
  • FIG10 is a schematic diagram of motion states of an embodiment of a collaborative robot arm provided by the present application at multiple moments;
  • FIG11 is a schematic diagram of the output torque value of the joint A of the collaborative robot arm during the movement of FIG10 ;
  • FIG12 is a schematic flow chart of a second embodiment of a collision detection method provided by the present application.
  • FIG13 is a schematic diagram of a flow chart of a third embodiment of a collision detection method provided by the present application.
  • FIG14 is a schematic diagram of the hardware structure of an embodiment of a collaborative robot arm provided by the present application.
  • FIG15 is a schematic diagram of the hardware structure of an industrial robot according to an embodiment of the present application.
  • FIG. 16 is a schematic diagram of the structure of an embodiment of a computer storage medium provided in the present application.
  • the motion control method and collision detection method of the present application are applied to a collaborative robotic arm, or a motion control system including a collaborative robotic arm, etc. In other embodiments, they can also be applied to robots in other fields or with other functions, such as industrial robots, sweeping robots, tracking robots and other articulated robots. In a specific embodiment, the motion control method and collision detection method of the present application can also be applied to a collaborative robotic arm, or a motion control system including a collaborative robotic arm.
  • Figure 1 is a flow chart of the first embodiment of the motion control method provided by the present application
  • Figure 2 is a hardware structure diagram of an embodiment of a collaborative robot arm in the motion control system provided by the present application
  • Figure 3 is a flow chart of an embodiment of the motion control method provided by the present application
  • Figure 4 is a hardware structure diagram of an embodiment of a motion control system provided by the present application
  • Figure 5 is a hardware structure diagram of another embodiment of the motion control system provided by the present application.
  • the collaborative robot arm 3 provided in the present application belongs to a specific motion control system, wherein the motion control system includes a host computer 1, a servo controller 2, and a collaborative robot arm 3.
  • the host computer 1 is connected in communication with the servo controller 2, and the servo controller 2 is connected in communication with the collaborative robot arm 3.
  • the collaborative robot arm 3 specifically includes a plurality of joints, such as a first joint 32 , a second joint 33 and a third joint 34 .
  • the plurality of joints are respectively connected to the servo controller 2 through the built-in interface 31 for communication.
  • the first joint 32 includes a servo driver 321 , a joint servo motor 322 , and an encoder 323 ;
  • the second joint 33 includes a servo driver 331 , a joint servo motor 332 , and an encoder 333 ;
  • the third joint 34 includes a servo driver 341 , a joint servo motor 342 , and an encoder 343 .
  • the servo driver 321 , the servo driver 331 , and the servo driver 341 are all connected to the servo controller 2 through the interface 31 , and the servo driver 341 is used to drive the corresponding joint servo motor 342 .
  • the collaborative robot arm 3 includes 6 or 7 joints.
  • the motion control system where the collaborative robot arm 3 is located is configured with a motion control function when working.
  • Its motion control function specifically includes the motion control method shown in Figures 1 and 3, or Figures 1 and 9.
  • the specific process please refer to the specific process of the motion control method applied to the motion control system, which will not be repeated here.
  • the control system where the collaborative robot arm 3 is located is configured with a collision detection function when working.
  • Its collision detection function specifically includes a collision detection method as shown in Figures 3 and 8, or Figures 8 and 9.
  • the specific process please continue to refer to the collision detection method applied to the motion control system, which will not be repeated here.
  • the motion control system adopts position-type closed-loop PID regulation.
  • a position-type closed-loop proportional-integral control module (hereinafter referred to as PID control module) may be provided for regulation.
  • the motion control method to be implemented in this application requires simultaneous operation and interaction of multiple terminals to realize a complete motion control method.
  • the following uses the motion control systems shown in Figures 4 and 5 as the execution subjects to further describe the control process of the motion control system.
  • the motion control method in this embodiment specifically includes the following steps:
  • Step S11 Calculate the theoretical joint motion state of the joint according to the motion control task.
  • the host computer in the motion control system receives the motion control instruction input by the user, and sends the motion control instruction to the servo controller.
  • the servo controller in the motion control system receives the motion control instruction sent by the host computer, and determines the motion control task of the collaborative robot arm by parsing the motion control instruction.
  • the motion control instruction at least includes the motion state of the collaborative robot arm specified by the user, such as the speed and direction during the motion process, and the specific position and posture of the motion end point, etc.
  • the motion control system determines the motion control task of the collaborative robot arm according to the motion control instruction, that is, the specific motion process.
  • the motion control task includes the motion planning state of the collaborative robot arm at each moment.
  • the motion control system performs motion planning according to the motion control task determined by the motion control instruction input by the user, and obtains the theoretical joint motion state corresponding to each joint of the collaborative robot arm at each motion moment.
  • motion planning can be carried out by the user specifying the final posture of the collaborative robot arm in the motion control task, and automatically calculating the theoretical motion state of each joint of the collaborative robot arm according to the motion planning algorithm, including but not limited to: at least one or more of the joint angle, joint velocity and joint acceleration of each joint in the robot.
  • motion planning can also be done by the user specifying the final path of the collaborative robot arm from multiple preset motion paths, thereby determining the final posture of the collaborative robot arm, and automatically calculating the theoretical motion state of each joint of the collaborative robot arm according to the motion planning algorithm.
  • the user inputs a motion control instruction on the host computer, and the host computer sends it to the servo controller.
  • the servo controller analyzes the motion control instruction, determines the motion control task, and then calculates the theoretical motion state of the joint according to the motion control task.
  • the motion control instruction is a motion trajectory.
  • the servo controller After receiving the motion trajectory, the servo controller parses it in the coordinate system of the collaborative robot arm to obtain the motion control task that the collaborative robot arm can perform.
  • the motion control instruction is a specified spatial point. After the servo controller receives the spatial point, it calibrates it in the coordinate system of the collaborative robot arm, and calculates the motion trajectory of the collaborative robot arm's current position to move to the spatial point based on the motion control algorithm. This motion trajectory is the motion control task that can be directly executed by the collaborative robot arm.
  • Step S12 Calculate the theoretical torque of the joint servo motor according to the theoretical motion state of the joint.
  • the motion control system calculates the theoretical torque of each joint servo motor and determines the adjustment torque threshold according to the theoretical motion state of each joint.
  • the motion control system inputs the theoretical joint motion state of step S11 into a preset inverse dynamics model.
  • the theoretical joint motion state includes the joint angle, joint velocity and joint acceleration of each joint.
  • the inverse dynamics model the theoretical joint torque of each joint of the collaborative robot arm can be calculated.
  • the motion control system uses an inverse dynamics model to calculate the theoretical motion state of the joints to obtain a torque column vector, and each vector element of the torque column vector corresponds to the theoretical joint torque of each joint of the collaborative robot arm, that is, the torque column vector is a collection of the theoretical joint torques of each joint of the collaborative robot arm.
  • the inertia moment vector is obtained by using the robot inertia matrix and acceleration column vector in the inverse dynamics model;
  • the gravity torque vector is obtained by using the gravity model in the inverse dynamics model and the joint angles of each joint;
  • the column vector used in the inverse dynamics model is the theoretical joint motion state of all joints of the collaborative robot arm, that is, the theoretical joint motion state of all joints of the collaborative robot arm is formed into a set in the form of a column vector, and then uniformly input into the inverse dynamics model for calculation. During the calculation process, the theoretical joint motion state of each joint in the column vector is independent of each other and does not affect each other.
  • the friction model in the inverse dynamics model and the joint velocities of each joint are used to obtain the friction torque vector, that is, the viscous friction function and Coulomb friction function in the friction model are used to process the velocities of each joint to obtain the vector elements in the friction torque vector.
  • ⁇ j and q are both column vectors, and the length of the column vector is equal to the number of joints of the collaborative robot arm; ⁇ j is the theoretical joint torque of each joint calculated by the inverse dynamics model, and q is the rotation angle of each joint. is the velocity of each joint, is the acceleration of each joint, M(q) is the robot inertia matrix, is the Coriolis force and centrifugal force matrix, G(q) is the moment caused by gravity, is the friction model.
  • the friction model used in this embodiment is: Among them, Fv is the viscous friction coefficient, Fs is the Coulomb friction coefficient, and sign() is the sign function, indicating the positive and negative signs of the velocities of each joint.
  • the motion control system uses the inverse dynamics model to calculate the theoretical motion state of each joint of the collaborative robot arm.
  • each joint of the collaborative robot arm actually rotates according to the reducer driven by the joint servo motor, and the angle of rotation of the output shaft of the reducer is the joint angle.
  • the motion control system also needs to calculate the theoretical motion state of each joint of the collaborative robot arm to obtain the theoretical torque of the joint servo motor of each joint.
  • the theoretical torque of the joint servo motor is the relevant data for directly controlling the operation of the joint.
  • the motion control system needs to consider the conditions of each joint reducer when calculating the theoretical torque of each joint servo motor, so as to obtain the accurate theoretical torque of the joint servo motor.
  • the motion control system calculates the theoretical joint torque of each joint in the collaborative robot arm based on the theoretical joint motion state, and calculates the theoretical torque of the servo motor of each joint of the collaborative robot arm according to the ratio of the theoretical joint torque of each joint of the collaborative robot arm and the reduction ratio.
  • the reduction ratio of each joint can be factory set and preset in the servo driver of each joint of the collaborative robot arm, or the reduction ratio of all joints can be preset in the servo controller.
  • the motion control system calculates the theoretical torque of each joint servo motor through the joint theoretical torque of each joint and the reduction ratio of each joint reducer.
  • the theoretical torque is used to calculate the current limit range of the joint servo motor.
  • R gear is the reduction ratio of each joint reducer
  • ⁇ j is the theoretical torque of each joint
  • ⁇ m is the theoretical torque of each joint servo motor.
  • Step S13 Calculate the current limit range of the joint servo motor according to the theoretical torque.
  • the servo controller in the motion control system sends the motor theoretical motion state and theoretical torque of the joint servo motor of each joint, and the adjustment torque threshold of each joint servo motor to the servo driver of each joint.
  • the servo driver of each joint receives the motor theoretical motion state, theoretical torque and adjustment torque threshold of the joint servo motor sent by the servo controller; and calculates the current limit range according to the theoretical torque and adjustment torque threshold of the joint servo motor.
  • the servo controller in the motion control system sends the theoretical joint motion state of each joint, the theoretical motor motion state and theoretical torque of each joint servo motor to the servo driver of each joint.
  • the servo driver of each joint receives the theoretical joint motion state of the joint, the theoretical torque of the joint servo motor and the adjustment torque threshold sent by the servo controller; and calculates the current limit range according to the theoretical torque of the joint servo motor and the adjustment torque threshold.
  • the theoretical torque of each joint can be characterized by the numerical value of the theoretical torque, or it can be the ratio of the theoretical torque of each joint servo motor to the rated torque of each joint servo motor, such as expressed in percentage, thousandths, etc., where the rated torque is the rated torque of each joint servo motor itself.
  • the motion control system calculates the current limit range according to the preset adjustment torque threshold of each joint servo motor.
  • the preset adjustment torque threshold of each joint servo motor can be pre-set and stored in the collaborative robot arm, and the adjustment torque threshold of each joint servo motor can be different, and for each joint servo motor, the adjustment torque threshold is a fixed value, and its value is related to the safety gear position of the joint.
  • the corresponding adjustment torque threshold is smaller, so the range of current output constraint is smaller, ensuring the safety of equipment and personnel.
  • the motion control system determines the torque constraint upper limit and the torque constraint lower limit according to the theoretical torque of each joint servo motor and the adjustment torque threshold;
  • ⁇ m is the adjustment torque threshold of each joint servo motor
  • ⁇ m is the theoretical torque of each joint servo motor
  • k T is the torque constant of each joint servo motor
  • Step S14 Control the drive current output to the joint servo motor so as not to exceed the current limit range.
  • Each joint servo driver in the motion control system uses the joint theoretical motion state as the input of the PID control module, determines the driving current of the joint servo motor, and limits the driving current according to the current limit range.
  • the motion control system constrains and outputs the driving current of each joint servo motor according to the current limit range of each joint servo motor.
  • the motion control system calculates the driving current output to the joint servo motor according to the theoretical motion state of the motor, constrains and outputs the driving current according to the current limit range determined in step S13, thereby controlling the joint movement and limiting the size of each driving current. This can prevent the collaborative robot arm from colliding. Due to PID adjustment, the output instantaneous torque increases, aggravating the damage to equipment and obstacles caused by the collision, thereby effectively protecting the safety of the collaborative robot arm, personnel, and materials.
  • the motion control system controls the drive current output to each joint servo motor to constrain the following manner:
  • the motion control system does not constrain the driving current of each joint servo motor.
  • the output drive current value is 0.6A.
  • the drive current output needs to be adjusted according to the current constraint upper limit, and the adjusted drive current is determined to be 0.8A and output.
  • the drive current output of the PID control module is 0.3A, it is judged that the drive current output is lower than the lower limit of the current limit range, then the drive current output needs to be adjusted according to the current constraint lower limit, and the adjusted drive current is determined to be 0.4A and output.
  • i ref is the driving current of each joint servo motor
  • ⁇ m is the theoretical torque of each joint servo motor
  • ⁇ m is the adjustment torque threshold of each joint servo motor
  • k T is the torque constant of each joint servo motor.
  • the motion control system can ensure that the drive current of the servo motor of each joint will be limited to In the range.
  • the motion control system may further adjust the driving current by adjusting a proportional coefficient or other preset adjustment methods.
  • the torque threshold and the torque constant of the motor are fixed values, and the theoretical torque of each joint servo motor is an instantaneous value, that is, the calculated current limit range should also be an instantaneous value. Therefore, the current limit range changes with the theoretical torque of each joint servo motor.
  • the motion control system calculates the theoretical motion state of the joint according to the motion control task; calculates the theoretical torque of the joint servo motor according to the theoretical motion state of the joint; calculates the current limit range of the joint servo motor according to the theoretical torque; and controls the drive current output to the joint servo motor not to exceed the current limit range.
  • the above method can directly limit the drive current of each joint by using the theoretical motion state of the joint and the theoretical torque of each joint servo motor, effectively control the output torque of each joint, reduce the damage caused by the collaborative robot arm to the collision obstacle or person in the event of a collision, and effectively protect the collaborative robot arm and the person.
  • Figure 6 is a flow chart of the second embodiment of the motion control method provided by the present application.
  • the motion control method in this embodiment is specifically applied to a servo controller, such as the servo controller in the motion control system shown in Figure 5.
  • the motion control method of this embodiment specifically includes the following steps:
  • Step S21 Calculate the theoretical joint motion state of the joint according to the motion control task.
  • the host computer receives the motion control instruction input by the user and sends the motion control instruction to the servo controller.
  • the servo controller receives the motion control instruction sent by the host computer and determines the motion control task of the collaborative robot arm by parsing the motion control instruction.
  • the servo controller performs motion planning according to the motion control task determined by the motion control instruction input by the user, and obtains the theoretical joint motion state corresponding to each joint of the collaborative robot arm at each motion moment.
  • Step S22 Calculate the theoretical torque of the joint servo motor according to the theoretical motion state of the joint.
  • the servo controller calculates the theoretical torque of the servo motor of each joint and determines the adjustment torque threshold according to the theoretical motion state of each joint.
  • the servo controller inputs the theoretical joint motion state of step S21 into a preset inverse dynamics model, and the theoretical joint torque of each joint of the collaborative robot arm can be calculated through the inverse dynamics model.
  • the motion control system uses the inverse dynamics model to calculate the theoretical motion state of each joint of the collaborative robot arm.
  • each joint of the collaborative robot arm rotates according to the reducer driven by the joint servo motor, and the angle of rotation of the output shaft of the reducer is the joint angle.
  • the motion control system also needs to calculate the theoretical motion state of each joint of the collaborative robot arm to obtain the theoretical torque of the joint servo motor of each joint, and the theoretical torque of the joint servo motor is the relevant data for directly controlling the operation of the joint.
  • the reducer plays the role of matching the speed and transmitting torque.
  • the influence of the reducer on the output torque needs to be considered.
  • the servo controller calculates the theoretical joint torque of each joint in the collaborative robot arm based on the theoretical joint motion state. Then, the servo controller obtains the reduction ratio of each joint of the collaborative robot arm, and calculates the theoretical torque of the servo motor of each joint of the collaborative robot arm based on the theoretical joint torque and reduction ratio of each joint of the collaborative robot arm.
  • Step S23 Send the theoretical torque to the corresponding servo driver; wherein the theoretical torque is used by the servo driver to calculate the current limit range of the joint servo motor and control the drive current output to the joint servo motor not to exceed the current limit range.
  • the servo controller sends the theoretical torque of each joint servo motor to the servo driver of each corresponding joint.
  • the theoretical torque sent to each joint servo driver is used to calculate the current limit range of the joint servo motor, and control the driving current output by each joint servo driver to the joint servo motor not to exceed the current limit range.
  • the servo controller can also send the theoretical torque of each joint servo motor, the theoretical motion state of the joint and the adjustment torque threshold of each joint to the servo driver of each joint, so that the servo driver of each joint calculates the current limit range according to the theoretical torque of the joint servo motor and the adjustment torque threshold, and calculates the driving current according to the theoretical motion state of the joint. Finally, the driving current is adjusted according to the current limit range to ensure that the adjusted driving current does not exceed the current limit range.
  • the adjustment torque threshold can be pre-set and stored in the servo controller, or stored in the host computer, and sent to the servo controller by the host computer.
  • the adjustment torque threshold may also be pre-stored in the servo driver of each joint. In this case, the servo controller only needs to send the theoretical torque of the servo motor of each joint and the theoretical motion state of the joint.
  • Figure 7 is a flow chart of the third embodiment of the motion control method provided by the present application.
  • the motion control method in this embodiment is specifically applied to a servo driver, such as the servo driver of any joint in the motion control system shown in Figure 5.
  • the motion control method of this embodiment specifically includes the following steps:
  • Step S31 receiving the theoretical torque of the joint servo motor.
  • the servo driver receives the theoretical motion state of the joint, the theoretical torque of the joint servo motor, and the adjustment torque threshold value sent by the servo controller.
  • the servo driver receives the theoretical joint motion state of the joint and the theoretical torque of the joint servo motor sent by the servo controller, and extracts the pre-stored adjustment torque threshold from the local storage space.
  • the servo controller sends the theoretical motion state of the joint and the theoretical torque of the joint servo motor to the servo driver of the corresponding joint.
  • the servo controller sends the theoretical torque ⁇ m of each joint servo motor as a guide value to the servo driver that controls the joint movement.
  • the theoretical torque ⁇ m of each joint servo motor and other data are generally transmitted through a communication bus. Commonly used bus protocols include CAN bus, EtherCAT bus, etc. In some embodiments, there is no specific restriction on the bus protocol of the transmitted communication bus.
  • the servo driver receives the theoretical joint motion state of the joint sent by the servo control, extracts the pre-stored adjustment torque threshold from the local storage space, and calculates the theoretical motor motion state of the joint servo motor according to the theoretical joint motion state.
  • Step S32 Calculate the current limit range according to the theoretical torque.
  • the servo driver calculates the current limit range according to the theoretical torque of the joint servo motor and the adjustment torque threshold.
  • the servo driver determines the torque constraint upper limit and the torque constraint lower limit based on the theoretical torque of the servo motor of the joint and the adjustment torque threshold; determines the current output constraint upper limit based on the torque constraint upper limit and the preset torque constant; determines the current output constraint lower limit based on the torque constraint lower limit and the preset torque constant; determines the current limiting range according to the current output constraint upper limit and the current output constraint lower limit.
  • Step S33 Control the driving current output to the joint servo motor so as not to exceed the current limit range.
  • the servo driver uses the theoretical motion state of the joint as the input of the PID control module, determines the driving current of the joint servo motor, and limits the driving current according to the current limiting range.
  • the servo driver receives the motor theoretical motion state of the joint servo motor sent by the servo controller, and the motor theoretical motion state is calculated by the servo controller according to the joint theoretical motion state.
  • the servo driver inputs the motor theoretical motion state into the PID control module, calculates the driving current output to the joint servo motor, and constrains the driving current according to the current limit range so that the driving current output to the joint servo motor does not exceed the current limit range.
  • the servo controller calculates the theoretical motion state of the joint servo motor according to the theoretical motion state of the joint, and sends the theoretical motion state of the motor to the servo driver.
  • the servo controller sends the theoretical motion state of the joint to the servo driver, so that the servo driver calculates the theoretical motion state of the motor according to the theoretical motion state of the joint.
  • the servo driver obtains the actual rotation angle of the servo motor of the joint, and obtains the theoretical rotation angle state of the servo motor of the joint according to the theoretical motion state of the motor. Then, the servo driver calculates the difference between the actual rotation angle of the servo motor of the joint and the theoretical rotation angle state corresponding to the current moment, and inputs the difference into the position closed-loop PID control module, so as to adjust the drive current output to the joint servo motor through the position closed-loop PID control module, and control the adjusted drive current not to exceed the current limit range.
  • the servo driver inputs the actual rotation angle and theoretical rotation angle of the joint servo motor into the PID control module to adjust the drive current output to the joint servo motor through the PID control module to control the adjusted drive current not to exceed the current limit range.
  • the servo driver can also calculate the drive current output to the servo motor of the joint through the position closed-loop PID control module. In some embodiments, the servo driver uses the motor theoretical motion state as the input of the position closed-loop PID control module to determine the drive current output to the servo motor of the joint.
  • the servo driver when the driving current output to the joint servo motor is within the current limit range, the servo driver does not adjust the driving current of the joint servo motor.
  • the servo driver adjusts the driving current according to the upper limit of the current output constraint to obtain the adjusted driving current output.
  • the servo driver adjusts the driving current according to the lower limit of the current output constraint to obtain the adjusted driving current output.
  • Figure 8 is a flow chart of the first embodiment of the collision detection method provided by the present application
  • Figure 9 is a flow chart of another embodiment of the motion control method provided by the present application
  • Figure 10 is a schematic diagram of the motion state of an embodiment of a collaborative robot arm provided by the present application at multiple times.
  • the collision detection method of this embodiment specifically includes the following steps:
  • Step S41 Calculate the theoretical joint motion state of the joint according to the motion control task.
  • Step S42 Calculate the theoretical torque of the joint servo motor and the theoretical motion state of the motor according to the theoretical motion state of the joint.
  • Step S43 Calculate the current limit range of the joint servo motor according to the theoretical torque.
  • Step S44 Control the drive current output to the joint servo motor so as not to exceed the current limit range.
  • step S41 to step S44 have been described in detail in step S11 to step S14 in the above embodiment, and will not be repeated here.
  • Step S45 Acquire the actual motion state of the joint servo motor.
  • the joint servo motor in the motion control system receives the driving current, drives the motor rotor to rotate, and the encoder detects the actual rotation angle of the motor rotor and sends the actual rotation angle of the motor rotor to the servo driver.
  • the motion control system can determine the actual motion state of the joint servo motor according to the actual position of the motor rotor.
  • the encoder of the joint servo motor sends the actual rotation angle of the motor rotor to the servo driver, and the servo driver obtains the position, that is, the actual rotation angle.
  • the servo driver calculates the difference between the actual rotation angle and the theoretical rotation angle corresponding to the current moment of the theoretical motion state of the motor, that is, calculates the tracking error, and then compares the tracking error with the preset collision threshold, and determines whether the collaborative robot arm has collided based on the comparison result. When the tracking error is greater than the collision threshold, the motion control system determines that the collaborative robot arm has collided.
  • the motion control system drives the motor rotor of the joint servo motor to rotate according to the driving current calculated and determined according to steps S41 to S44.
  • the encoder in the joint servo motor detects the actual rotation angle of the motor rotor and sends the actual rotation angle of the motor rotor to the servo driver, thereby enabling the motion control system to obtain the actual motion state of the joint servo motor.
  • FIG. 10 and 11 there are structural schematic diagrams of the position states of the collaborative robot arm at times t1-t4 during the movement, and a schematic diagram of the output torque value of the joint A of the collaborative robot arm.
  • the horizontal axis represents time
  • the vertical axis represents the output torque value of the joint A
  • the "constrained” solid line represents the output torque value of the joint A in the present application
  • the "unconstrained” dotted line represents the output torque value of the joint A in the prior art.
  • joint A of the collaborative robot arm is located at point a and is planned to move to point b. After the collaborative robot arm has been running for a period of time.
  • joint A moves to point c and collides with obstacle B.
  • the collaborative robot arm has not yet detected the collision and is ready to continue running to point b along the predetermined route.
  • the torque values represented by the dotted line and the solid line are basically equal.
  • the joint A of the collaborative robot arm cannot move due to the obstruction of obstacle B.
  • the actual rotation angle of the joint servo motor corresponding to joint A is theoretically at point d at the moment t3 (the moment t3' is the same as the moment t3, and the moment t3' refers to the position of joint A at the moment t3' when the collaborative robot arm is operating normally).
  • the servo driver will actively adjust through the PID control module to compensate for the error position, increase the output current of the joint servo motor, and then increase the corresponding output torque of joint A. Therefore, after the moment t3, the output torque of joint A increases rapidly.
  • the output torque (unit: N ⁇ m) of joint A will quickly increase to the maximum value (i.e., max value) that joint A itself can output. Although it is still unable to move, it will instantly output a larger torque, aggravating the damage to the equipment and the object being hit.
  • the upper limit of the output torque of joint A when a collision occurs is limited, that is, the maximum value of the output torque at this time is reduced, so that when it is determined that a collision has occurred at time t4, the output torque value of joint A will not exceed the upper threshold value.
  • an output current limit range is set for the joint servo motor, which can constrain the output torque of the collaborative robot arm and prevent the collaborative robot arm from outputting a large force and causing great harm to obstacles or people.
  • the obstacle B prevents the collaborative robot arm from moving according to the planned path, resulting in the actual rotation angle of the motor rotor of the collaborative robot arm (joint A) being unable to continue moving after time t2.
  • the motion control system can detect the actual rotation angle of the motor rotor of the joint servo motor through the encoder, and intuitively detect the difference between the actual motion state and the theoretical motion state of each joint of the collaborative robot arm.
  • the actual motion state of the joint servo motor includes but is not limited to: the actual rotation angle and actual speed of the joint servo motor.
  • Step S46 Calculate the tracking error according to the actual motion state and the motor theoretical motion state of the joint servo motor.
  • the motion control system calculates the tracking error based on the actual motion state and the state data of the same state type in the theoretical motion state of the joint servo motor, such as angle state data or speed state data.
  • the motion control system can extract the actual rotation angle of the joint servo motor in the actual motion state of the joint servo motor, and extract the theoretical rotation angle of the joint servo motor corresponding to the moment of the actual rotation angle in the theoretical motion state of the joint servo motor; then, determine the angle tracking error based on the difference between the actual rotation angle and the theoretical rotation angle.
  • the motion control system can extract the actual speed of the joint servo motor in the actual motion state of the joint servo motor, and extract the theoretical speed of the joint servo motor in the motor theoretical motion state of the joint servo motor; then, according to the difference between the actual speed and the theoretical speed, the speed tracking error is determined.
  • the actual speed of the joint motor rotor can be calculated by the speed of the actual rotation angle change of the joint motor rotor.
  • the tracking error of the motion control system may include a combination of one or more of the above-mentioned angle tracking error and speed tracking error.
  • Step S47 When the tracking error exceeds the collision threshold, it is determined that the collaborative robot arm has collided.
  • the motion control system calculates the real-time tracking error and the preset collision threshold by comparing step S46. When the tracking error exceeds the collision threshold, it can be determined that the collaborative robot arm has collided.
  • the actual rotation angle ⁇ actual and the actual speed ⁇ actual of the motor rotor are limited due to the output torque being limited.
  • ⁇ ref the theoretical rotation angle
  • ⁇ ref the theoretical rotation angle
  • speed tracking error the error that is generally called angle tracking error and speed tracking error.
  • the specific collision detection method is as follows:
  • the motion control system determines the angle tracking error based on the difference between the actual angle and the theoretical angle of each joint; when the angle tracking error exceeds the collision threshold, it is determined that the collaborative robot arm has collided.
  • the motion control system determines a speed tracking error based on the difference between the actual speed and the theoretical speed of each joint; when the speed tracking error exceeds a collision threshold, it is determined that a collision occurs in the collaborative robot arm.
  • the motion control system may determine that the collaborative robot arm has collided when any one of the conditions of determining that the speed tracking error exceeds the collision threshold and the angle tracking error exceeds the collision threshold is met.
  • the collaborative robot arm when encounters an obstacle during movement, it can detect the occurrence of a collision by judging the size of the tracking error of the angle/speed, so that the joint can stop safely.
  • the staff can configure the adjustment torque threshold ⁇ m , the angle tracking error excessive alarm threshold ⁇ and the speed tracking error excessive alarm threshold according to different loads, robot movement speeds and actual safety requirements of human-machine collaboration. For example, when the angular load is set large, the system is configured with a larger ⁇ m ; when the collaborative robot moves faster, a larger ⁇ and When the safety requirement for human-machine collaboration is high, a smaller ⁇ m should be configured.
  • the torque threshold ⁇ m , the angle tracking error excessive alarm threshold ⁇ and the speed tracking error excessive alarm threshold are adjusted. It can also be automatically configured based on preset configuration rules, load, robot movement speed and actual safety requirements of human-robot collaboration.
  • the motion control system stops supplying power to the servo motors of each joint to make the servo motors of each joint lose power and stop rotating, and stops supplying power to the brakes of each joint to make the brakes of each joint lose power and brake, thereby enabling the collaborative robot arm to stop in time after a collision is determined to occur.
  • the motion control system can also control each joint to perform a retraction action.
  • the retraction action of the joint can be: each joint moves a distance in the opposite direction of the motion direction indicated by the theoretical motion state of the motor.
  • the motion control system controls each joint to perform a retracting action, and then stops supplying power to the servo motors of each joint and to the brakes of each joint, so that the servo motors of each joint lose power and stop rotating, and the brakes of each joint lose power and engage.
  • the motion control system monitors the actual motion state of the joint through the encoder of the joint servo motor, and detects whether the collaborative robot arm collides according to the tracking error between the actual motion state and the theoretical motion state.
  • the output torque of each joint of the collaborative robot arm is effectively controlled.
  • the damage caused by the collaborative robot arm to the obstacle or person in the collision is reduced, and the collaborative robot arm and the person are effectively protected.
  • FIG 12 is a flow chart of the second embodiment of the collision detection method provided by the present application.
  • the collision detection method in this embodiment is specifically applied to a servo controller, such as the servo controller in the motion control system shown in Figure 5.
  • the collision detection method in this embodiment specifically includes the following steps:
  • Step S51 Calculate the theoretical joint motion state of the joint according to the motion control task.
  • Step S52 Calculate the theoretical torque of the joint servo motor and the theoretical motion state of each joint servo motor according to the theoretical motion state of the joint, wherein the theoretical torque is used by the servo driver to calculate the current limit range of the joint servo motor and control the drive current output to the joint servo motor not to exceed the current limit range.
  • Step S53 Send the theoretical torque and the theoretical motion state of the motor to the corresponding servo driver.
  • steps S51 to S53 have been described in detail in steps S21 to S23 in the above embodiment, and will not be repeated here.
  • Step S54 Acquire the actual motion state of the joint servo motor.
  • the servo controller obtains the actual rotation angle of the motor rotor detected by the encoder of the joint servo motor through the servo driver, and determines the actual motion state of the joint servo motor using the actual position of the motor rotor.
  • Step S55 Calculate the tracking error according to the actual motion state and the motor theoretical motion state of the joint servo motor.
  • the servo controller calculates the tracking error according to the same state type of the actual motion state and the motor theoretical motion state of the joint servo motor.
  • the servo controller obtains the actual rotation angle of the joint servo motor at the current moment, and obtains the theoretical rotation angle of the joint servo motor corresponding to the current moment in the motor theoretical motion state of the joint servo motor, and determines the angle tracking error based on the difference between the actual rotation angle and the theoretical rotation angle.
  • the servo controller extracts the actual speed of the joint servo motor in the actual motion state of the joint servo motor, and extracts the theoretical speed of the joint servo motor in the motor theoretical motion state of the joint servo motor, and determines the speed tracking error according to the difference between the actual speed and the theoretical speed.
  • the actual speed of the joint motor rotor is calculated by the speed of the actual rotation angle change of the joint motor rotor.
  • the tracking error of the servo controller may include a combination of one or more of the above-mentioned angle tracking error and speed tracking error.
  • Step S56 When the tracking error exceeds the collision threshold, it is determined that the collaborative robot arm has collided, and a stop command is sent to the servo driver.
  • the servo controller calculates the real-time tracking error and the preset collision threshold by comparing step S56. When the tracking error exceeds the collision threshold, it is determined that a collision occurs at the joint.
  • the servo controller determines that a collision has occurred, on the one hand, it can send a stop command to the servo drivers of all joints to make all joints stop moving according to the stop command; on the other hand, it can send a collision report to the host computer so that the host computer issues a collision warning to inform the staff that the collaborative robot arm has collided at this time.
  • the host computer is provided with an LED light, and the host computer emits light of a specified color through the LED or flashes at a specified frequency by controlling the LED light to issue a collision warning.
  • the host computer is provided with a speaker, and the host computer emits a specified sound through the speaker to issue a collision warning.
  • the motion control system performs the following steps:
  • S101 The host computer receives a motion control instruction input by a user
  • S102 The host computer sends a motion control instruction to the servo controller
  • the servo controller receives a motion control instruction sent by the host computer
  • S202 The servo controller parses the motion control instruction and determines the motion control task
  • S203 The servo controller calculates the theoretical joint motion state of each joint according to the motion control task
  • the servo controller calculates the theoretical torque and the theoretical motion state of the servo motor of each joint according to the theoretical motion state of each joint, and determines the adjustment torque threshold;
  • the servo controller sends the motor theoretical motion state, theoretical torque and adjustment torque threshold of each joint servo motor to each joint servo driver;
  • the servo driver receives the motor theoretical motion state, theoretical torque and adjustment torque threshold of the joint servo motor sent by the servo controller;
  • the servo driver calculates a current limit range according to the theoretical torque of the joint servo motor and the adjustment torque threshold;
  • the servo driver uses the theoretical motion state of the motor as the input of the PID control module, calculates the driving current of the joint servo motor, and limits the driving current according to the current limit range;
  • the joint servo motor receives the driving current, drives the motor rotor to rotate, and the encoder detects the actual rotation angle of the motor rotor and sends the actual rotation angle of the motor rotor to the servo driver;
  • S311 The servo driver receives the abnormal actual rotation angle of the motor rotor
  • S312 The servo driver compares the actual rotation angle of the abnormal motor rotor with the theoretical rotation angle of the motor rotor in the theoretical motion state of the motor, and calculates the tracking error;
  • S313 The servo driver compares whether the tracking error is greater than the collision threshold
  • S3151 The servo driver sends a collision signal of the joint to the servo controller
  • S501 The brake of the collision joint loses power and brakes
  • the servo controller receives a collision signal of the joint sent by the servo driver
  • S2131 The servo controller generates a stop command and sends it to the servo drivers of all other non-collision joints;
  • S321 The servo drivers of all non-collision joints receive a stop command
  • S2132 The servo controller sends a collision report to the host computer
  • S501 The brakes of all non-collision joints lose power and hold the brakes
  • S111 The host computer receives the collision report
  • S112 The host computer issues a collision warning.
  • the motion control system performs the following steps:
  • S101 The host computer receives a motion control instruction input by a user
  • S102 The host computer sends a motion control instruction to the servo controller
  • the servo controller receives a motion control instruction sent by the host computer
  • S202 The servo controller parses the motion control instruction and determines the motion control task
  • S203 The servo controller calculates the theoretical joint motion state of each joint according to the motion control task
  • the servo controller calculates the theoretical torque and the theoretical motion state of the servo motor of each joint according to the theoretical motion state of each joint, and determines the adjustment torque threshold;
  • S205 The servo controller sends the motor theoretical motion state, theoretical torque, and adjustment torque threshold of each joint servo motor to each joint servo driver;
  • the servo driver receives the motor theoretical motion state, theoretical torque and adjustment torque threshold of the joint servo motor sent by the servo controller;
  • the servo driver calculates a current limit range according to the theoretical torque of the joint servo motor and the adjustment torque threshold;
  • the servo driver uses the theoretical motion state of the motor as the input of the PID control module, calculates the driving current of the joint servo motor, and limits the driving current according to the current limit range;
  • the joint servo motor receives the driving current, drives the motor rotor to rotate, and the encoder detects the actual rotation angle of the motor rotor and sends the actual rotation angle of the motor rotor to the servo driver;
  • S311 The servo driver receives the actual rotation angle of the abnormal motor rotor and sends it to the servo controller;
  • the servo controller receives the abnormal actual rotation angle of the motor rotor
  • S212 The servo controller compares the actual rotation angle of the abnormal motor rotor with the theoretical rotation angle of the motor rotor in the theoretical motion state, and calculates the tracking error;
  • S213 The servo controller compares whether the tracking error is greater than the collision threshold
  • S2151 The servo controller sends a stop command to the servo driver of the joint;
  • S2152 The servo controller sends a collision report to the host computer
  • S111 The host computer receives the collision report
  • S112 The host computer issues a collision warning.
  • the technical solution of the present application in the process of controlling the movement of the collaborative robot arm, calculates the current limit range according to the theoretical motion state of the collaborative robot arm, and controls the driving current of the servo motors of each joint of the collaborative robot arm not to exceed the current limit range; the driving current of each joint is directly limited by utilizing the theoretical motion state of the joint and the theoretical torque of the servo motors of each joint, thereby effectively controlling the external output torque of each joint, reducing the damage caused by the collaborative robot arm to the colliding obstacles or people in the event of a collision, and effectively protecting the collaborative robot arm and people.
  • the following table shows an example of parameters related to a collaborative robot arm and industrial safety.
  • the output torque increase range of the joint during collision can be limited, so that when the collaborative robot arm collides, the maximum collision and clamping pressure caused to the collision object is only 200N, which is much smaller than the 1000N collision and clamping pressure generated when the collaborative robot arm outputs the maximum output torque under normal working conditions, and is lower than the maximum safe collision pressure of 210N that the human body can withstand, which can effectively improve the safety of the collaborative robot arm during operation.
  • the main purpose of the technical solution of this application is to reduce the damage caused to the human body when a collision occurs, not to completely avoid the occurrence of damage.
  • the current output constraint upper limit and the current output constraint lower limit of the current limit range are set according to the adjustment torque threshold.
  • the adjustment torque threshold can be sent by the user through the host computer, so as to conveniently set the current limit range according to the user's needs.
  • the torque threshold can also be adjusted according to the safety gear of the current environment or application scenario.
  • the current limit range can be flexibly set according to different safety requirements, which can meet the different needs of the collaborative robot arm in different usage environments or application scenarios.
  • FIG 13 is a schematic flow chart of the third embodiment of the collision detection method provided by the present application.
  • the collision detection method in this embodiment is specifically applied to a servo driver, such as the servo driver of any joint in the motion control system shown in Figure 5.
  • the collision detection method in this embodiment specifically includes the following steps:
  • Step S61 receiving the motor theoretical motion state and theoretical torque of the joint servo motor.
  • Step S62 Calculate the current limit range according to the theoretical torque.
  • Step S63 Control the driving current output to the servo joint servo motor so as not to exceed the current limit range.
  • Step S64 receiving the actual motion state of the joint servo motor.
  • the servo driver obtains the actual rotation angle of the motor rotor detected by the encoder of the joint servo motor, and determines the actual motion state of the joint servo motor using the real-time position of the motor rotor.
  • Step S65 Calculate the tracking error according to the motor theoretical motion state and actual motion state of the joint servo motor.
  • Step S66 When the tracking error exceeds the collision threshold, it is determined that a collision occurs at the joint.
  • Figure 14 is a schematic diagram of the hardware structure of an embodiment of the collaborative robot arm provided by the present application.
  • the collaborative robot arm 700 of this embodiment includes a processor 71 , a memory 72 , an input/output device 73 , and a bus 74 .
  • the processor 71, memory 72, and input/output device 73 are respectively connected to a bus 74.
  • the memory 72 stores program data
  • the processor 71 is used to execute the program data to implement the motion control method and/or collision detection method of the above-mentioned embodiment.
  • the processor 71 may also be referred to as a CPU (Central Processing Unit).
  • the processor 71 may be an integrated circuit chip having the ability to process signals.
  • the processor 71 may also be a general-purpose processor, a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field programmable gate array (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, or discrete hardware components.
  • DSP digital signal processor
  • ASIC application-specific integrated circuit
  • FPGA field programmable gate array
  • a general-purpose processor may be a microprocessor or the processor 71 may also be any conventional processor, etc.
  • FIG. 15 is a schematic diagram of the hardware structure of an embodiment of the industrial robot provided by the present application.
  • the industrial robot 800 of this embodiment includes a processor 81 , a memory 82 , an input/output device 83 , and a bus 84 .
  • the processor 81 , the memory 82 , and the input/output device 83 are respectively connected to the bus 84 .
  • the memory 82 stores program data.
  • the processor 81 is used to execute the program data to implement the motion control method and/or collision detection method of the above-mentioned embodiment.
  • FIG. 16 is a structural diagram of an embodiment of a computer storage medium provided by the present application.
  • the computer storage medium 900 stores program data 91.
  • the program data 91 is executed by the processor, it is used to implement the motion control method/or collision detection method of the above-mentioned embodiment.
  • the embodiments of the present application When the embodiments of the present application are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium.
  • the technical solution of the present application is essentially or partly contributed to the prior art or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including several instructions to enable a computer device (which can be a personal computer, server, or network device, etc.) or a processor (processor) to perform all or part of the steps of the method described in each embodiment of the present application.
  • the aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk and other media that can store program code.

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)

Abstract

本申请提供了一种协作机械臂及其运动控制方法、碰撞检测方法、控制系统、工业机器人以及计算机存储介质。该运动控制方法包括：根据运动控制任务，计算关节的关节理论运动状态；根据关节理论运动状态，计算关节伺服电机的理论力矩；根据理论力矩，计算关节伺服电机的电流限制范围；控制输出给关节伺服电机的驱动电流不超出电流限制范围。上述方法可以利用关节理论运动状态以及各个关节伺服电机的理论力矩直接对各个关节的驱动电流进行限制，有效控制各个关节对外的输出力矩大小，减少协作机械臂在发生碰撞时产生的伤害，有效保护协作机械臂和人。

Description

协作机械臂及其运动控制方法、碰撞检测方法、控制系统 技术领域
本申请涉及机器人控制技术领域，特别是涉及一种协作机械臂及其运动控制方法、碰撞检测方法、控制系统、工业机器人以及计算机存储介质。
背景技术
协作机械臂是指被设计成可以在协作区域内与人直接进行交互的机器人，其具有快速部署、人机协作安全等传统工业所不具有的优势。安全作为协作机械臂的主要特征与优势，表现在当与人或者环境发生碰撞时，协作机械臂要能够快速识别碰撞的信号并采取策略避免造成进一步的碰撞伤害。
目前的协作机械臂在运动过程中与障碍物发生碰撞后，会输出更大的电流继续驱动协作机械臂，以使协作机械臂能越过障碍物按预定的轨迹继续运动；而在电流从正常运动的电流值逐渐增加的期间，协作机械臂持续对障碍物施加力，直至电流达到一个较大的电流阈值，甚至该电流阈值为该关节的额定电流，或者检测到碰撞信号后，才会对电流限制，在这个过程中协作机械臂会对障碍物造成较大的损害；尤其是当障碍物是人的时候，协作机械臂会给人造成较大的伤害；同时，对协作机械臂本身也会带来不小的损害。
申请内容
本申请提供了一种协作机械臂及其运动控制方法、碰撞检测方法、控制系统、工业机器人以及计算机存储介质，主要解决的技术问题是如何限制协作机械臂输出的驱动电流以及输出力矩，减小协作机械臂在发生碰撞时产生的伤害。
为解决上述技术问题，本申请提供了一种协作机械臂的运动控制方法，包括：
根据所述运动控制任务，计算关节的关节理论运动状态；
根据所述关节理论运动状态，计算关节伺服电机的理论力矩；
根据所述理论力矩，计算所述关节伺服电机的电流限制范围；
控制输出给所述关节伺服电机的驱动电流不超出所述电流限制范围。
为解决上述技术问题，本申请提供了另一种协作机械臂的运动控制 方法，所述运动控制方法应用于伺服控制器，包括：
根据所述运动控制任务，计算关节的关节理论运动状态；
根据所述关节理论运动状态，计算关节伺服电机的理论力矩；
将所述理论力矩发送给伺服驱动器；
其中，所述理论力矩用于供所述伺服驱动器计算关节伺服电机的电流限制范围，并控制输出给所述关节伺服电机的驱动电流不超出电流限制范围。
为解决上述技术问题，本申请提供了另一种协作机械臂的运动控制方法，所述运动控制方法应用于任意一个关节的伺服驱动器，包括：
接收关节伺服电机的理论力矩；
根据所述理论力矩，计算电流限制范围；
控制输出给关节伺服电机的驱动电流不超出所述电流限制范围。
为解决上述技术问题，本申请提供了一种协作机械臂的运动控制系统，包括上位机、伺服控制器、多个关节的伺服驱动器以及多个关节伺服电机；
所述上位机与所述伺服控制器通信连接，所述伺服控制器分别与各个所述伺服驱动器通信连接，各个所述伺服驱动器用于驱动对应的关节伺服电机；
所述协作机械臂的运动控制系统，在工作时配置有运动控制功能，所述运动控制功能包括：
如上述的运动控制方法。
为解决上述技术问题，本申请提供了一种协作机械臂的碰撞检测方法，所述碰撞检测方法包括：
根据运动控制任务，计算关节的关节理论运动状态；
根据所述关节理论运动状态，计算关节伺服电机的理论力矩和关节伺服电机的电机理论运动状态；
根据所述理论力矩，计算所述关节伺服电机的电流限制范围；
控制输出给所述关节伺服电机的驱动电流不超出所述电流限制范围；
获取所述关节伺服电机的实际运动状态；
根据所述实际运动状态和所述电机理论运动状态，计算跟踪误差；
当所述跟踪误差超过碰撞阈值时，判定所述协作机械臂发生碰撞。
为解决上述技术问题，本申请提供了另一种协作机械臂的碰撞检测方法，所述碰撞检测方法应用于一种伺服控制器，包括：
根据运动控制任务，计算关节的关节理论运动状态；
根据所述关节理论运动状态，计算关节伺服电机的理论力矩和电机理论运动状态；
将所述理论力矩和所述电机理论运动状态发送给对应的伺服驱动 器；
获取所述关节伺服电机的实际运动状态；
根据所述实际运动状态和所述电机理论运动状态，计算跟踪误差；
当所述跟踪误差超过碰撞阈值时，判定所述协作机械臂发生碰撞，并发送停机指令给所述伺服驱动器；
其中，所述理论力矩用于所述伺服驱动器计算所述关节伺服电机的电流限制范围，并控制输出给所述关节伺服电机的驱动电流不超出所述电流限制范围。
为解决上述技术问题，本申请提供了另一种协作机械臂的碰撞检测方法，所述碰撞检测方法应用于任意一个关节的伺服驱动器，包括：
接收关节伺服电机的理论力矩和电机理论运动状态；
根据所述理论力矩，计算电流限制范围；
控制输出给所述关节伺服电机的驱动电流不超出所述电流限制范围；
接收所述关节伺服电机的实际运动状态；
根据所述关节伺服电机的电机理论运动状态和实际运动状态，计算跟踪误差；
当所述跟踪误差超过碰撞阈值时，判定所在关节发生碰撞。
为解决上述技术问题，本申请提供了一种协作机械臂的控制系统，包括上位机、伺服控制器、多个关节的伺服驱动器以及多个关节伺服电机；
所述上位机与所述伺服控制器通信连接，所述伺服控制器分别与各个所述关节的伺服驱动器通信连接，各个所述关节的伺服驱动器用于驱动对应的关节伺服电机；
所述协作机械臂的控制系统，在工作时配置有碰撞检测功能，所述碰撞检测功能包括：
如上述的碰撞检测方法。
为解决上述技术问题，本申请还提供了一种协作机械臂，所述协作机械臂包括处理器以及与所述处理器耦接的存储器；
其中，所述存储器用于存储程序数据，所述处理器用于执行所述程序数据以实现上述的运动控制方法和/或上述的碰撞检测方法。
为解决上述技术问题，本申请还提供了一种工业机器人，所述工业机器人包括处理器以及与所述处理器耦接的存储器；
其中，所述存储器用于存储程序数据，所述处理器用于执行所述程序数据以实现上述的运动控制方法和/或上述的碰撞检测方法。
为解决上述技术问题，本申请还提供了一种计算机存储介质，所述计算机存储介质用于存储程序数据，所述程序数据在被处理器执行时，用以实现上述的运动控制方法和/或上述的碰撞检测方法。
与现有技术相比，本申请中：运动控制系统根据运动控制任务，计算关节的关节理论运动状态；根据关节理论运动状态，计算关节伺服电机的理论力矩；根据理论力矩，计算关节伺服电机的电流限制范围；控制输出给关节伺服电机的驱动电流不超出电流限制范围。上述方法可以利用关节理论运动状态以及各个关节伺服电机的理论力矩直接对各个关节的驱动电流进行限制，有效控制各个关节对外的输出力矩大小，减少协作机械臂在发生碰撞时对碰撞的障碍物或人产生的伤害，有效保护协作机械臂和人。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。其中：
图1是本申请提供的运动控制方法第一实施例的流程示意图；
图2是本申请提供的运动控制系统中协作机械臂一实施例的硬件结构示意图；
图3是本申请提供的运动控制方法一实施例的流程示意图；
图4是本申请提供的运动控制系统一实施例的硬件结构示意图；
图5是本申请提供的运动控制系统另一实施例的硬件结构示意图；
图6是本申请提供的运动控制方法第二实施例的流程示意图；
图7是本申请提供的运动控制方法第三实施例的流程示意图；
图8是本申请提供的碰撞检测方法第一实施例的流程示意图；
图9是本申请提供的运动控制方法另一实施例的流程示意图；
图10是本申请提供协作机械臂的一实施例在多个时刻的运动状态示意图；
图11是协作机械臂在图10的运动过程中的关节A的输出力矩值示意图；
图12是本申请提供的碰撞检测方法第二实施例的流程示意图；
图13是本申请提供的碰撞检测方法第三实施例的流程示意图；
图14是本申请提供的协作机械臂一实施例的硬件结构示意图；
图15是本申请提供的工业机器人一实施例的硬件结构示意图；
图16是本申请提供的计算机存储介质一实施例的结构示意图。
具体实施方式
下面将结合在本实施例中的附图，对在本实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本申请的一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在 没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
本申请的运动控制方法以及碰撞检测方法应用于一种协作机械臂，或者包括协作机械臂的运动控制系统等，在其他实施例中，也可以应用于其他领域或者其他功能的机器人，例如工业机器人、扫地机器人、循迹机器人等其他的关节机器人。在一种具体的实施方式中，本申请的运动控制方法以及碰撞检测方法还可以应用于一种协作机械臂，以及包括协作机械臂的运动控制系统。
其中，下面先介绍本申请的运动控制方法以及碰撞检测方法所涉及的部分关键技术术语：
Figure PCTCN2022138465-appb-000001
基于上述技术基础，本申请提出一种应用于运动控制系统的运动控制方法，具体请参见图1至图5，图1是本申请提供的运动控制方法第一实施例的流程示意图，图2是本申请提供的运动控制系统中协作机械臂一实施例的硬件结构示意图，图3是本申请提供的运动控制方法一实施例的流程示意图，图4是本申请提供的运动控制系统一实施例的硬件结构示意图，图5是本申请提供的运动控制系统另一实施例的硬件结构示意图。
如图4和图5所示，本申请提供的协作机械臂3属于一种具体的运动控制系统，其中，运动控制系统包括上位机1、伺服控制器2以及协作机械臂3。上位机1与伺服控制器2通信连接，而伺服控制器2与协作机械臂3通信连接。
在一些实施例中，如图5所示，协作机械臂3具体包括多个关节，例如第一关节32、第二关节33以及第三关节34。其中，多个关节分别通过内置接口31与伺服控制器2通信连接。
第一关节32包括伺服驱动器321、关节伺服电机322、编码器323，第二关节33包括伺服驱动器331、关节伺服电机332、编码器333，第三关节34包括伺服驱动器341、关节伺服电机342、编码器343。
其中，伺服驱动器321、伺服驱动器331、伺服驱动器341均通过接口31与伺服控制器2通信连接，且伺服驱动器341用于驱动对应的关节伺服电机342。
在一些实施例中，协作机械臂3包括6或7个关节。
具体地，协作机械臂3所在的运动控制系统，在工作时配置有运动控制功能，其运动控制功能具体包括如图1和图3、或图1和图9所示的运动控制方法，其具体过程请参阅应用于运动控制系统的运动控制方法的具体过程，在此不再赘述。
在一些实施例中，协作机械臂3所在的控制系统，在工作时配置有碰撞检测功能，其碰撞检测功能具体包括如图3和图8、或图8和图9所示的碰撞检测方法，其具体过程请继续参阅应用于运动控制系统的碰撞检测方法，在此不再赘述。
在本实施中，运动控制系统采用位置式闭环PID调节，具体的，可以为设置有位置式闭环比例微积分控制模块(下称PID控制模块)进行调节。
如图3所示，本申请所要实现的运动控制方法需要多端的同时运行和交互才能实现完整的运动控制方法，下面分别以图4和图5所示的运动控制系统作为执行主体，进一步描述运动控制系统的控制过程。
如图1所示，在本实施例的运动控制方法具体包括以下步骤：
步骤S11：根据运动控制任务，计算关节的关节理论运动状态。
在本实施例中，请参阅图3中的S101至S102，以及S201至S202。如S101和S102所示，运动控制系统中的上位机接收用户输入的运动控制指令，并将运动控制指令发送给伺服控制器。如S201和S202所示，运动控制系统中的伺服控制器接收上位机发送的运动控制指令，通过解析该运动控制指令，确定协作机械臂的运动控制任务。
具体地，运动控制指令中至少包括用户指定协作机械臂的运动状态，例如运动过程中的速度、方向，以及运动终点的具体位姿等。运动控制系统根据运动控制指令确定协作机械臂的运动控制任务，即具体的运动过程，运动控制任务包括协作机械臂每一时刻的运动规划状态。
在本实施例中，请参阅图3中的S203。运动控制系统根据用户输入的运动控制指令确定的运动控制任务进行运动规划，得到协作机械臂每一运动时刻对应各个关节的关节理论运动状态。
其中，运动规划可以由运动控制任务中用户指定协作机械臂的最终位姿，按照运动规划算法自动计算协作机械臂的各个关节的关节理论运动状态，包括但不限于：机器人中各个关节的关节转角、关节速度以及关节加速度中的至少一种或多种。
在一些实施例中，运动规划也可以由用户从多条预设的运动路径中指定协作机械臂的最终路径，从而确定协作机械臂的最终位姿，按照运动规划算法自动计算协作机械臂的各个关节的关节理论运动状态。
在本实施例中，用户在上位机上输入运动控制指令，上位机发送给伺服控制器。伺服控制器接收该运动指令后，解析该运动控制指令，确定运动控制任务，再根据运动控制任务，计算关节的关节理论运动状态。
在一此实施例中，运动控制指令是一条的运动轨迹，伺服控制器接收到该运动轨迹后，在协作机械臂的坐标系中进行解析，得到协作机械臂可以执行的运动控制任务。
在一些实施例中，运动控制指令是一个指定的空间点，伺服控制器接收到该空间点后，在协作机械臂的坐标系中标定出来，并根据运动控制算法计算得到协作机械臂的当前位置移动到该空间点的运动轨迹，该运动轨迹即可以被协作机械臂直接执行的运动控制任务。
步骤S12：根据关节理论运动状态，计算关节伺服电机的理论力矩。
在本实施例中，请参阅图3中的S204。运动控制系统根据各个关节的关节理论运动状态，计算各个关节伺服电机的理论力矩和确定调节力矩阈值。
具体地，运动控制系统将步骤S11的关节理论运动状态输入预设的逆动力学模型，关节理论运动状态包括各个关节的关节转角、关节速度以及关节加速度，通过逆动力学模型，能够计算出协作机械臂各个关节的关节理论力矩。
在一些实施例中，运动控制系统利用逆动力学模型对关节理论运动 状态进行运算，得到力矩列向量，力矩列向量的各个向量元素分别对应于协作机械臂各个关节的关节理论力矩，即力矩列向量为协作机械臂各个关节的关节理论力矩的集合。
逆动力学模型的具体运算过程如下：
利用逆动力学模型中的机器人惯量矩阵和加速度列向量，获取惯量力矩向量；
利用逆动力学模型中的科氏力与离心力模型和速度列向量，获取科氏力与离心力力矩向量；
利用逆动力学模型中的重力模型和各个关节的关节转角，获取重力力矩向量；
将惯量力矩向量、科氏力与离心力力矩向量、重力力矩向量以及摩擦力力矩向量的相应向量元素相加，进而确定力矩列向量，以及协作机械臂各个关节的关节理论力矩。
在一些实施例中，逆动力学模型中所使用的列向量为协作机械臂所有关节的关节理论运动状态，即将协作机械臂所有关节的关节理论运动状态通过列向量的方式形成一个集合，从而统一输入到逆动力学模型进行运算。在运算过程中，列向量中的各个关节的关节理论运动状态相互独立，互不影响。
在一些实施例中，利用逆动力学模型中的摩擦力模型和各个关节的关节速度，获取摩擦力力矩向量，即利用摩擦力模型中的粘性摩擦函数和库伦摩擦函数对各个关节速度进行处理，得到摩擦力力矩向量中的各个向量元素。
运动控制系统所采用的逆动力学模型的具体表现公式如下：
Figure PCTCN2022138465-appb-000002
公式中的τ j，q都是列向量，列向量的长度与协作机械臂的关节数量相等；τ j是通过逆动力学模型计算的各个关节的关节理论力矩，q是各个关节转角，
Figure PCTCN2022138465-appb-000003
是各个关节速度，
Figure PCTCN2022138465-appb-000004
是各个关节加速度，M(q)是机器人惯量矩阵，
Figure PCTCN2022138465-appb-000005
是科氏力与离心力矩阵，G(q)是重力产生的力矩，
Figure PCTCN2022138465-appb-000006
是摩擦力模型。
在本实施例使用的摩擦力模型是：
Figure PCTCN2022138465-appb-000007
其中，F v是粘性摩擦系数，F s是库伦摩擦系数，sign()是符号函数，指出各个关节速度的正负号。
在一些实施例中，运动控制系统采用逆动力学模型计算所得的是协作机械臂各个关节的关节理论运动状态，在协作机械臂的实际控制中，协作机械臂的各个关节实际上是根据关节伺服电机驱动减速机转动，减速机的输出轴所转动的转角即为关节转角。
因此，运动控制系统还需要将协作机械臂各个关节的关节理论运动 状态计算得到各个关节的关节伺服电机的理论力矩，而关节伺服电机的理论力矩才是直接控制关节运行的相关数据。
因此，运动控制系统在计算各个关节伺服电机的理论力矩时需要考虑各个关节减速机的情况，从而得到准确的关节伺服电机的理论力矩。
在一些实施例中，运动控制系统基于上述的关节理论运动状态，计算协作机械臂中各个关节的关节理论力矩。根据协作机械臂各个关节的关节理论力矩和减速比的比值关系，计算协作机械臂各个关节伺服电机的理论力矩。
在一些实施例中，各个关节的减速比可以出厂设置，预设在协作机械臂的各个关节的伺服驱动器，也可以将所有关节的减速比均预设在伺服控制器。
在本实施例中，运动控制系统通过各个关节的关节理论力矩、各个关节减速机的减速比，计算出各个关节伺服电机的理论力矩，理论力矩用于计算关节伺服电机的电流限制范围。
具体地，通过计算各个关节的关节理论力矩和减速比的比值，得到各个关节伺服电机的理论力矩，计算公式如下：
Figure PCTCN2022138465-appb-000008
其中，R gear为各个关节减速机的减速比，τ j为各个关节的关节理论力矩，τ m为各个关节伺服电机的理论力矩。
步骤S13：根据理论力矩，计算关节伺服电机的电流限制范围。
在本实施例中，请参阅图3中的S205，以及S301至S302。运动控制系统中的伺服控制器发送各个关节的关节伺服电机的电机理论运动状态和理论力矩，及各个关节伺服电机的调节力矩阈值给各个关节的伺服驱动器。而各个关节的伺服驱动器接收伺服控制器发送的该关节伺服电机的电机理论运动状态、理论力矩及调节力矩阈值；根据关节伺服电机的理论力矩和调节力矩阈值计算电流限制范围。
在一些实施例中，运动控制系统中的伺服控制器发送各个关节的关节理论运动状态、各个关节伺服电机的电机理论运动状态和理论力矩给各个关节的伺服驱动器。各个关节的伺服驱动器接收伺服控制器发送的该关节的关节理论运动状态、关节伺服电机的理论力矩及调节力矩阈值；根据关节伺服电机的理论力矩和调节力矩阈值计算电流限制范围。
具体地，各个关节的关节理论力矩可以通过理论力矩的数值进行表征，也可以为各个关节伺服电机的理论力矩与各个关节伺服电机的额定力矩的比值，如以百分比、千分比等表现形式，额定力矩为各个关节伺服电机自身的额定力矩。
例如，具体的关节伺服电机的理论力矩数值为1.98N·m，而对应关 节的关节伺服电机的额定力矩为19.8N·m时，关节伺服电机的理论力矩的千分比表现结果为100‰。
需要说明的是，后续描述中的调节力矩阈值、输出电流等均可以采用不同的表现形式，在此不一一列举。
在本实施例中，运动控制系统根据各个关节伺服电机的预设的调节力矩阈值，计算电流限制范围。
其中，各个关节伺服电机的预设的调节力矩阈值，可预先设置并存储在协作机械臂，而每个关节伺服电机的调节力矩阈值可以不同，且对于每个关节伺服电机而言，调节力矩阈值为一固定值，其取值与关节的安全档位相关。
例如，当某一关节的安全档位越高，其对应的调节力矩阈值越小，从而对电流输出约束的范围就越小，保证设备与人身安全。
在一些实施例中，运动控制系统根据各个关节伺服电机的理论力矩与调节力矩阈值确定力矩约束上限和力矩约束下限；
基于力矩约束上限与转矩常数确定电流输出约束上限；
基于力矩约束下限与转矩常数确定电流输出约束下限；
按照电流输出约束上限和电流输出约束下限，确定电流限制范围，计算公式如下：
Figure PCTCN2022138465-appb-000009
其中，Δτ m为各个关节伺服电机的调节力矩阈值、τ m为各个关节伺服电机的理论力矩、k T为各个关节伺服电机的转矩常数。
步骤S14：控制输出给关节伺服电机的驱动电流不超出电流限制范围。
在本实施例中，请参阅图3中的S303。运动控制系统中的各个关节伺服驱动器将关节理论运动状态作为PID控制模块的输入，确定关节伺服电机的驱动电流，并根据电流限制范围对驱动电流进行限制。
具体地，运动控制系统按照各个关节伺服电机的电流限制范围对各个关节伺服电机的驱动电流进行约束并进行输出。
在一些实施例中，运动控制系统按照电机理论运动状态计算输出给关节伺服电机的驱动电流，按照步骤S13确定的电流限制范围对驱动电流进行约束并进行输出，从而控制关节运动，同时对各驱动电流大小进行限制，能够避免协作机械臂在发生碰撞，由于PID调节，导致输出的瞬间力矩增大，加重碰撞对设备及障碍物的伤害，有效保护协作机械臂及人身、物料的安全。
在一些实施例中，运动控制系统控制输出给各个关节伺服电机的驱动电流进行约束的方式如下：
当输出给关节伺服电机的驱动电流在电流限制范围之内时，运动控 制系统不对各个关节伺服电机的驱动电流进行约束。
当输出给关节伺服电机的驱动电流超出电流输出约束上限
Figure PCTCN2022138465-appb-000010
时，按照电流输出约束上限
Figure PCTCN2022138465-appb-000011
对驱动电流进行调整，得到调整后的驱动电流输出。
当输出给关节伺服电机的驱动电流低于电流输出约束下限
Figure PCTCN2022138465-appb-000012
时，按照电流输出约束下限
Figure PCTCN2022138465-appb-000013
对驱动电流进行调整，得到调整后的驱动电流输出。
例如，运动控制系统计算得到的电流限制范围为0.4A至0.8A，而PID控制模块输出的驱动电流输出为0.6A时，则不需要对驱动电流输出进行调整，输出的驱动电流的电流值为0.6A。
例如，运动控制系统计算得到的电流限制范围为0.4A至0.8A，而PID控制模块输出的驱动电流输出为1.0A时，则需要对驱动电流输出进行按照电流约束上限进行调整，确定调整后的驱动电流为0.8A并进行输出。
又例如，若PID控制模块输出的驱动电流输出为0.3A时，判断驱动电流输出低于电流限制范围的下限，则需要对驱动电流输出进行按照电流约束下限进行调整，确定调整后的驱动电流为0.4A并进行输出。
上述驱动电流输出的具体调整流程如下：
Figure PCTCN2022138465-appb-000014
时，i ref不用被调整；
Figure PCTCN2022138465-appb-000015
时，
Figure PCTCN2022138465-appb-000016
Figure PCTCN2022138465-appb-000017
时，
Figure PCTCN2022138465-appb-000018
其中，i ref为各个关节伺服电机的驱动电流，τ m为各个关节伺服电机的理论力矩，Δτ m为各个关节伺服电机的调节力矩阈值，k T为各个关节伺服电机的转矩常数。
通过以上调整过程，运动控制系统可以确保各个关节伺服电机的驱动电流会被限制在
Figure PCTCN2022138465-appb-000019
的范围内。
在一些实施例中，运动控制系统对驱动电流的调整方式还可以通过调整比例系数或其他预设的调节方式进行调节。
在一些实施例中，调节力矩阈值和电机的转矩常数为固定值，而各个关节伺服电机的理论力矩为瞬时值，即计算得到的电流限制范围同样应当为瞬时值。因此，电流限制范围随着各个关节伺服电机的理论力矩变化而变化。
在本实施例中，运动控制系统根据运动控制任务，计算关节的关节 理论运动状态；根据关节理论运动状态，计算关节伺服电机的理论力矩；根据理论力矩，计算关节伺服电机的电流限制范围；控制输出给关节伺服电机的驱动电流不超出电流限制范围。上述方法可以利用关节理论运动状态以及各个关节伺服电机的理论力矩直接对各个关节的驱动电流进行限制，有效控制各个关节对外的输出力矩大小，减少协作机械臂在发生碰撞时对碰撞的障碍物或人产生的伤害，有效保护协作机械臂和人。
下面以运动控制系统中的伺服控制器作为执行主体继续阐述图1所示的运动控制方法：
请继续参阅图6，图6是本申请提供的运动控制方法第二实施例的流程示意图。在本实施例的运动控制方法具体应用于一种伺服控制器，如图5所示运动控制系统中的伺服控制器。
如图6所示，本实施例的运动控制方法具体包括以下步骤：
步骤S21：根据运动控制任务，计算关节的关节理论运动状态。
在本实施例中，请参阅图3中的S101至S102，以及S201至S203。
如S101和S102所示，上位机接收用户输入的运动控制指令，并将运动控制指令发送给伺服控制器。如S201和S202所示，伺服控制器接收上位机发送的运动控制指令，通过解析该运动控制指令，确定协作机械臂的运动控制任务。
在本实施例中，请参阅图3中的S203。伺服控制器根据用户输入的运动控制指令确定的运动控制任务进行运动规划，得到协作机械臂每一运动时刻对应各个关节的关节理论运动状态。
步骤S22：根据关节理论运动状态，计算关节伺服电机的理论力矩。
在本实施例中，请参阅图3中的S204。伺服控制器根据各个关节的关节理论运动状态，计算各个关节伺服电机的理论力矩和确定调节力矩阈值。
伺服控制器将步骤S21的关节理论运动状态输入预设的逆动力学模型，通过逆动力学模型，能够计算出协作机械臂各个关节的关节理论力矩。
在一些实施例中，运动控制系统采用逆动力学模型计算所得的是协作机械臂各个关节的关节理论运动状态，在协作机械臂的实际控制中，协作机械臂的各个关节根据关节伺服电机驱动减速机转动，减速机的输出轴所转动的转角即为关节转角。
因此，运动控制系统还需要将协作机械臂各个关节的关节理论运动状态计算得到各个关节的关节伺服电机的理论力矩，而关节伺服电机的理论力矩是直接控制关节运行的相关数据。
其中，减速机起匹配转速和传递转矩的作用，电机在输出扭力时，需要考虑减速机对输出扭力的影响。
在一些实施例中，伺服控制器基于上述的关节理论运动状态，计算协作机械臂中各个关节的关节理论力矩。然后，伺服控制器获取协作机械臂各个关节的减速比，基于协作机械臂各个关节的关节理论力矩和减速比，计算协作机械臂各个关节伺服电机的理论力矩。
步骤S23：将理论力矩发送给对应的伺服驱动器；其中，理论力矩用于伺服驱动器计算关节伺服电机的电流限制范围，并控制输出给关节伺服电机的驱动电流不超出电流限制范围。
在本实施例中，请参阅图3中的S205，以及S301至S302。伺服控制器发送各个关节伺服电机的理论力矩给各个对应关节的伺服驱动器。其中，下发给各个关节伺服驱动器的理论力矩用于计算关节伺服电机的电流限制范围，并控制各个关节伺服驱动器输出给关节伺服电机的驱动电流不超出电流限制范围。
在一些实施例中，伺服控制器还可以将各个关节伺服电机的理论力矩、关节理论运动状态以及各个关节的调节力矩阈值下发到各个关节的伺服驱动器，以使各个关节的伺服驱动器根据关节伺服电机的理论力矩和调节力矩阈值计算电流限制范围，以及根据关节理论运动状态计算驱动电流，最后，根据电流限制范围对驱动电流进行调整，以保证调整后的驱动电流不超出电流限制范围，调节力矩阈值可以预先设置并存储在伺服控制器，或者存储在上位机，并由上位机下发到伺服控制器。
在一些实施例中，调节力矩阈值也可以预先存储在各个关节的伺服驱动器，此时，伺服控制器只需要下发各个关节伺服电机的理论力矩、关节理论运动状态即可。
下面以运动控制系统中各个关节的伺服驱动器作为执行主体继续阐述图1所示的运动控制方法：
请继续参阅图7，图7是本申请提供的运动控制方法第三实施例的流程示意图。在本实施例的运动控制方法具体应用于一种伺服驱动器，如图5所示运动控制系统中的任意一个关节的伺服驱动器。
如图7所示，本实施例的运动控制方法具体包括以下步骤：
步骤S31：接收关节伺服电机的理论力矩。
在本实施例中，请参阅图3中的S301，伺服驱动器接收伺服控制器发送的所在关节的关节理论运动状态、关节伺服电机的理论力矩以及调节力矩阈值。
在一些实施例中，伺服驱动器接收伺服控制器发送的所在关节的关节理论运动状态、关节伺服电机的理论力矩，并从本地存储空间提取预先存储的调节力矩阈值。
具体地，伺服控制器将关节理论运动状态以及关节伺服电机的理论力矩下发到相应关节的伺服驱动器。伺服控制器把各个关节伺服电机的 理论力矩τ m作为指导值下发给到控制关节运动的伺服驱动器，各个关节伺服电机的理论力矩τ m等数据一般通过通讯总线进行传输，常用的总线协议有CAN总线、EtherCAT总线等。在一些实施例中，对传输的通讯总线的总线协议不做具体限制。
在一些实施例中，伺服驱动器接收伺服控制发送的所在关节的关节理论运动状态，从本地存储空间提取预先存储的调节力矩阈值，同时根据关节理论运动状态计算得到关节伺服电机的电机理论运动状态。
步骤S32：根据理论力矩，计算电流限制范围。
在本实施例中，请参阅图3中的S302，伺服驱动器根据关节伺服电机的理论力矩和调节力矩阈值，计算电流限制范围。
在一些实施例中，伺服驱动器根据所在关节伺服电机的理论力矩与调节力矩阈值确定力矩约束上限和力矩约束下限；基于力矩约束上限与预设的转矩常数确定电流输出约束上限；基于力矩约束下限与预设的转矩常数确定电流输出约束下限；按照电流输出约束上限和电流输出约束下限，确定电流限制范围。
步骤S33：控制输出给关节伺服电机的驱动电流不超出电流限制范围。
在本实施例中，请参阅图3中的S303，伺服驱动器将关节理论运动状态作为PID控制模块的输入，确定关节伺服电机的驱动电流，并根据电流限制范围对驱动电流进行限制。
具体地，伺服驱动器接收伺服控制器发送的关节伺服电机的电机理论运动状态，电机理论运动状态为伺服控制器根据关节理论运动状态计算得到。伺服驱动器将电机理论运动状态输入PID控制模块，计算输出给关节伺服电机的驱动电流大小；根据电流限制范围对驱动电流进行约束，以使输出给关节伺服电机的驱动电流不超出电流限制范围。
在一些实施例中，伺服控制器根据关节理论运动状态，计算关节伺服电机的电机理论运动状态，并将电机理论运动状态发送给伺服驱动器。
在一些实施例中，伺服控制器将关节理论运动状态发送给伺服驱动器，以使所述伺服驱动器根据所述关节理论运动状态计算得到电机理论运动状态。
在一些实施例中，伺服驱动器获取所在关节伺服电机的实际转角，根据电机理论运动状态，获取所在关节伺服电机的理论转角状态。然后，伺服驱动器计算所在关节伺服电机的实际转角和理论转角状态中当前时刻对应的理论转角的差值，并将差值输入到位置式闭环PID控制模块，以通过位置式闭环PID控制模块对输出给关节伺服电机的驱动电流进行调整，且控制调整后的驱动电流不超出电流限制范围。
在一些实施例中，伺服驱动器将关节伺服电机的实际转角和理论转角输入到PID控制模块，以通过PID控制模块对输出给关节伺服电机的 驱动电流进行调整，控制调整后的驱动电流不超出电流限制范围。
在一些实施例中，伺服驱动器还可以通过位置式闭环PID控制模块算输出给所在关节伺服电机的驱动电流。在一些实施例中，伺服驱动器将电机理论运动状态作为位置式闭环PID控制模块的输入，从而确定输出给所在关节伺服电机的驱动电流。
在一些实施例中，在输出给关节伺服电机的驱动电流在电流限制范围时，伺服驱动器不对所在关节伺服电机的驱动电流进行调整。在输出给关节伺服电机的驱动电流超过电流输出约束上限时，伺服驱动器按照电流输出约束上限对驱动电流进行调整，得到调整后的驱动电流输出。在输出给关节伺服电机的驱动电流低于电流输出约束下限时，伺服驱动器按照电流输出约束下限对驱动电流进行调整，得到调整后的驱动电流输出。
基于上述技术基础和运动控制方法，本申请提出一种应用于运动控制系统的碰撞检测方法，具体请参阅图3、图8、图9和图10，图8是本申请提供的碰撞检测方法第一实施例的流程示意图，图9是本申请提供的运动控制方法另一实施例的流程示意图，图10是本申请提供协作机械臂的一实施例在多个时刻的运动状态示意图。
如图8所示，本实施例的碰撞检测方法具体包括以下步骤：
步骤S41：根据运动控制任务，计算关节的关节理论运动状态。
步骤S42：根据关节理论运动状态，计算关节伺服电机的理论力矩和电机理论运动状态。
步骤S43：根据理论力矩，计算关节伺服电机的电流限制范围。
步骤S44：控制输出给关节伺服电机的驱动电流不超出电流限制范围。
在本实施例中，步骤S41至步骤S44的具体技术内容在上述实施例中的步骤S11至步骤S14中已有详细阐述，在此不再赘述。
步骤S45：获取关节伺服电机的实际运动状态。
在一些实施例中，请参阅图3及图9中的S401和S411，运动控制系统中的关节伺服电机接收驱动电流，驱动电机转子转动，编码器检测电机转子的实际转角，发送电机转子的实际转角给伺服驱动器。本实施例中，运动控制系统可以根据电机转子的实际位置确定关节伺服电机的实际运动状态。
在一些实施例中，在发生碰撞的情况下，关节伺服电机的编码器发送电机转子的实际转角给伺服驱动器，伺服驱动器获得该位置，即实际转角，伺服驱动器根据该实际转角，与电机理论运动状态的当前时刻对应的理论转角，计算实际转角与理论转角的差值，即计算出跟踪误差，然后将跟踪误差与预设的碰撞阈值进行比较，根据比较结果判断协作机 械臂是否发生碰撞。当跟踪误差大于碰撞阈值，运动控制系统判定协作机械臂发生碰撞。
具体地，运动控制系统根据步骤S41至步骤S44计算确定的驱动电流驱动关节伺服电机的电机转子转动，关节伺服电机内的编码器通过检测电机转子的实际转角，并将电机转子的实际转角发送给伺服驱动器，从而使得运动控制系统获取关节伺服电机的实际运动状态。
如图10和图11所示，协作机械臂在运动过程中，分别在t1-t4时刻的位置状态的结构示意图，以及协作机械臂的关节A的输出力矩值的示意图，图11中的坐标系中，横轴表示时间，纵轴表示关节A的输出力矩值；“有约束”的实折线表示本申请中的关节A的输出力矩值，“无约束”的虚折线表示现有技术中的关节A的输出力矩值。
在本实施例中，在t1时刻，协作机械臂的关节A位于a点处，计划运动到b点处。当协作机械臂运行了一段时间后。
在t2时刻，关节A移动到c点，和障碍物B发生碰撞，此时尚未检测到协作机械臂发生了碰撞，准备继续按预定的路线运行到b点。
在t3时刻之前，虚折线和实折线所表示的力矩值基本相等。在t3时刻，协作机械臂的关节A由于受到障碍物B的阻拦，而无法进行移动，关节A对应的关节伺服电机的实际转角，在t3时刻，理论上关节A的运动位置在d点(t3`时刻与t3时刻是同一时刻，t3`时刻是指当协作机械臂正常运行时关节A在t3`时刻时的位置)，此时伺服驱动器会通过PID控制模块进行主动调节，补偿误差位置，会加大关节伺服电机的输出电流，进而增大对应的关节A的输出力矩，因此t3时刻之后，关节A的输出力矩瞬间迅速增大。
现有技术中，关节A的输出力矩(单位：N·m)会迅速增加到该关节A本身能够输出的最大值(即max值)，虽仍无法移动，但会在瞬时输出一个较大的力矩，加重设备和被撞物体的伤害。
在t4时刻判定协作机械臂发生了碰撞，从而停止或快速停止输出电流，对应的关节A的输出力矩会迅速下降，在t5时刻下降到0，从而抱闸停机。
在本实施例中，通过设置关节伺服电机的电流限制范围，进而限制了发生碰撞时关节A的输出力矩的上升上限，即降低了此时的输出力矩max值，从而当t4时刻判定发生了碰撞时，关节A的输出力矩值不会超过阈值上限。
本实施例的方案中，对关节伺服电机设置了输出的电流限制范围，可以使协作机械臂的输出的力矩进行约束，避免协作机械臂输出较大的力而对障碍物或人造成很大的伤害。
请继续参阅图10的协作机械臂运动状态对比图，关节A对应的电机转子转角的对比情况请继续参阅下表：
Figure PCTCN2022138465-appb-000020
由上表分析可知，在协作机械臂的关节A与障碍物B发生碰撞之后，由于障碍物B阻碍协作机械臂按照规划路径运动，导致协作机械臂(关节A)在t2时刻后电机转子的实际转角无法继续运动，运动控制系统可以通过编码器检测关节伺服电机的电机转子的实际转角，直观地检测出协作机械臂各个关节的实际运动状态与理论运动状态之间的差异。
其中，关节伺服电机的实际运动状态包括但不限于：关节伺服电机的实际转角、实际速度等。
步骤S46：根据实际运动状态和关节伺服电机的电机理论运动状态，计算跟踪误差。
在本实施例中，请参阅图3中的S311至S312，或者图9中的S311，以及S211至S212，运动控制系统根据实际运动状态和关节伺服电机的电机理论运动状态中同一状态类型的状态数据，例如转角状态数据或者速度状态数据等，计算跟踪误差。
在一些实施例中，运动控制系统可以提取关节伺服电机的实际运动状态中关节伺服电机的实际转角，以及提取关节伺服电机的电机理论运动状态中关节伺服电机的与该实际转角的时刻对应的理论转角；然后，根据实际转角和理论转角的差值，确定转角跟踪误差。
在一些实施例中，运动控制系统可以提取关节伺服电机的实际运动状态中关节伺服电机的实际速度，以及提取关节伺服电机的电机理论运动状态中关节伺服电机的理论速度；然后，根据实际速度和理论速度的差值，确定速度跟踪误差。其中，关节电机转子的实际速度可以通过关节电机转子的实际转角变化快慢计算。
综上，运动控制系统的跟踪误差可以包括上述的转角跟踪误差和速度跟踪误差中的一种或多种的组合。
步骤S47：当跟踪误差超过碰撞阈值时，判定协作机械臂发生碰撞。
在本实施例中，请参阅图3中的S313至S314，或者图9中的S213至S214，运动控制系统通过比较步骤S46计算实时的跟踪误差与预设的碰撞阈值，当跟踪误差超过碰撞阈值的情况下，即可判定协作机械臂发生碰撞。
在一些实施例中，当协作机械臂在运动过程中碰到障碍物时，由于输出扭力被限制，电机转子的实际转角θ actual、实际速度
Figure PCTCN2022138465-appb-000021
就会和理论转角θ ref、理论速度
Figure PCTCN2022138465-appb-000022
出现比较明显的误差，这个误差一般被称为转角跟踪误差和速度跟踪误差。通过判断转角跟踪误差或速度跟踪误差的大小，可以检测出协作机械臂是否发生了碰撞。
其具体的碰撞检测方法如下：
在一些实施例中，运动控制系统基于各个关节的实际转角和理论转角的差值，确定转角跟踪误差；在转角跟踪误差超过碰撞阈值时，判定协作机械臂发生碰撞。
在一些实施例中，运动控制系统基于各个关节的实际速度和理论速度的差值，确定速度跟踪误差；在速度跟踪误差超过碰撞阈值时，判定协作机械臂发生碰撞。
在一些实施例中，运动控制系统在确定速度跟踪误差超过碰撞阈值，和转角跟踪误差超过碰撞阈值中的任意一个条件满足时，即可判定协作机械臂发生碰撞。
上述碰撞检测过程的具体流程如下：
a.给各个关节预先设定触发转角跟踪误差过大报警的碰撞阈值Δθ和触发速度跟踪误差过大报警的碰撞阈值
Figure PCTCN2022138465-appb-000023
b.当|θ ref-θ actual|≥Δθ时，触发转角跟踪误差过大判定协作机械臂发生碰撞。
c.或当
Figure PCTCN2022138465-appb-000024
时，触发速度跟踪误差过大判定协作机械臂发生碰撞。
这样，当协作机械臂在运动过程中碰到障碍物时，就可以通过对转角/速度的跟踪误差大小的判断来检测碰撞的发生，使关节可以安全停下来。
在一些实施例中，工作人员可以根据不同的负载、机器人运动速度和人机协作的实际安全要求，配置调节力矩阈值Δτ m、转角跟踪误差过大报警的阈值Δθ和速度跟踪误差过大报警的阈值
Figure PCTCN2022138465-appb-000025
比如说当设置的角载较大时，系统对应的配置较大的Δτ m；当协作机械臂运动速度较快时，则要配置较大的Δθ和
Figure PCTCN2022138465-appb-000026
人机协作安全要求较高时，则要配置较小的Δτ m。
在一些实施例中，调节力矩阈值Δτ m、转角跟踪误差过大报警的阈值Δθ和速度跟踪误差过大报警的阈值
Figure PCTCN2022138465-appb-000027
还可以根据预设的配置规则，基于负载、机器人运动速度和人机协作的实际安全要求进行自动配置。
在一些实施例中，请继续参阅图3和图9中的S3221至S3222，在判定协作机械臂发生碰撞后，运动控制系统停止向各个关节伺服电机供电，以使各个关节伺服电机失电停转，并停止向各个关节的抱闸器供电，以使各个关节的抱闸器失电抱闸，从而使得协作机械臂能够在判定发生碰撞后及时停止。
在一些实施例中，运动控制系统还可以控制各个关节执行回退动作。具体地，关节的回退动作可以为：各个关节按照电机理论运动状态所指示的运动方向的反方向运动一段距离。
在一些实施例中，当判定协作机械臂发生碰撞时，运动控制系统控 制各个关节执行回退动作后，停止向各个关节伺服电机供电，并停止向各个关节的抱闸器供电，以使各个关节伺服电机失电停转、各个关节的抱闸器失电抱闸。
在本实施例中，协作机械臂在运动时，根据理论运动状态对协作机械臂的输出力矩进行约束；运动控制系统通过关节伺服电机的编码器监测所在关节的实际运动状态，根据实际运动状态与理论运动状态的跟踪误差进行检测协作机械臂是否发生碰撞。通过该碰撞检测方法，使协作机械臂在发生碰撞时，有效控制协作机械臂的各个关节对外的输出力矩大小，在碰撞检测过程中，减少在碰撞检测过程中协作机械臂在发生碰撞时对碰撞的障碍物或人产生的伤害，有效保护协作机械臂和人。
下面以运动控制系统中的伺服控制器作为执行主体继续阐述图8所示的碰撞检测方法：
请继续参阅图12，图12是本申请提供的碰撞检测方法第二实施例的流程示意图。在本实施例的碰撞检测方法具体应用于一种伺服控制器，如图5所示运动控制系统中的伺服控制器。
如图12所示，在本实施例的碰撞检测方法具体包括以下步骤：
步骤S51：根据运动控制任务，计算关节的关节理论运动状态。
步骤S52：根据关节理论运动状态，计算关节伺服电机的理论力矩以及各个关节伺服电机的电机理论运动状态，其中，理论力矩用于伺服驱动器计算关节伺服电机的电流限制范围，并控制输出给关节伺服电机的驱动电流不超出电流限制范围。
步骤S53：将理论力矩和电机理论运动状态发送给对应的伺服驱动器。
在本实施例中，步骤S51至步骤S53的具体技术内容在上述实施例中的步骤S21至步骤S23中已有详细阐述，在此不再赘述。
步骤S54：获取关节伺服电机的实际运动状态。
在本实施例中，请参阅图9中的S211，伺服控制器通过伺服驱动器获取关节伺服电机的编码器检测的电机转子的实际转角，利用电机转子的实际位置确定关节伺服电机的实际运动状态。
步骤S55：根据实际运动状态和关节伺服电机的电机理论运动状态，计算跟踪误差。
在本实施例中，请参阅图9中的S212，伺服控制器根据实际运动状态和关节伺服电机的电机理论运动状态的同一状态类型，计算跟踪误差。
在一些实施例中，伺服控制器获取关节伺服电机的当前时刻的实际转角，以及获取关节伺服电机的电机理论运动状态中关节伺服电机的与当前时刻对应的理论转角，根据实际转角和理论转角的差值，确定转角跟踪误差。
在一些实施例中，伺服控制器提取关节伺服电机的实际运动状态中 关节伺服电机的实际速度、以及提取关节伺服电机的电机理论运动状态中关节伺服电机的理论速度，根据实际速度和理论速度的差值，确定速度跟踪误差。关节电机转子的实际速度通过关节电机转子的实际转角变化快慢计算。
综上，伺服控制器的跟踪误差可以包括上述的转角跟踪误差和速度跟踪误差中的一种或多种的组合。
步骤S56：当跟踪误差超过碰撞阈值时，判定协作机械臂发生碰撞，并发送停机指令给伺服驱动器。
在在本实施例中，请参阅图9中的S213至S214，伺服控制器通过比较步骤S56计算实时的跟踪误差与预设的碰撞阈值，当跟踪误差超过碰撞阈值的情况下，即判定所在关节发生碰撞。
在一些实施例中，请继续参阅图9中的S2151至S2152，伺服控制器判定发生碰撞之后，一方面可以发送停机指令给所有关节的伺服驱动器，以使所有关节根据停止指令停止运动；另一方面可以发送碰撞报告给上位机，以使上位机发出碰撞警告，告知工作人员此时协作机械臂发生碰撞。
在一些实施例中，上位机通过显示屏上加载指定的画面以发出碰撞警告。
在一些实施例中，上位机设置有LED灯，上位机通过LED发出指定颜色的光或以通过控制LED灯指定的频率闪烁以发出碰撞警告。
在一些实施例中，上位机设置有扬声器，上位机通过扬声器发出指定的声音以发出碰撞警告。
参照图3，在一些实施例中，运动控制系统执行以下步骤：
S101：上位机接收用户输入的运动控制指令；
S102：上位机发送运动控制指令给伺服控制器；
S201：伺服控制器接收上位机发送的运动控制指令；
S202：伺服控制器解析运动控制指令，确定运动控制任务；
S203：伺服控制器根据运动控制任务，计算各关节的关节理论运动状态；
S204：伺服控制器根据各关节的关节理论运动状态，计算各关节伺服电机的理论力矩和电机理论运动状态，以及确定调节力矩阈值；
S205：伺服控制器发送各关节伺服电机的电机理论运动状态、理论力矩及调节力矩阈值给各关节伺服驱动器；
S301：伺服驱动器接收伺服控制器发送的该关节伺服电机的电机理论运动状态、理论力矩及调节力矩阈值；
S302：伺服驱动器根据关节伺服电机的理论力矩和调节力矩阈值计算电流限制范围；
S303：伺服驱动器将电机理论运动状态作为PID控制模块的输入， 计算关节伺服电机的驱动电流，并根据电流限制范围对驱动电流进行限制；
S401：关节伺服电机接收驱动电流，驱动电机转子转动，编码器检测电机转子的实际转角，发送电机转子的实际转角给伺服驱动器；
S411：关节伺服电机发送异常的电机转子的实际转角给伺服驱动器；
S311：伺服驱动器接收异常的电机转子的实际转角；
S312：伺服驱动器将异常的电机转子的实际转角与电机理论运动状态中电机转子的理论转角进行比较，计算跟踪误差；
S313：伺服驱动器比较跟踪误差是否大于碰撞阈值；
S314：若比较结果为是，则伺服驱动器判定发生碰撞；
S3151：伺服驱动器发送该关节的碰撞信号给伺服控制器；
S3152：伺服驱动器停止向抱闸器输出电流；
S3153：伺服驱动器停止向关节伺服电机输出电流；
S501：碰撞关节的抱闸器失电抱闸；
S421：碰撞关节的关节伺服电机失电停转；
S211：伺服控制器接收伺服驱动器发送的关节的碰撞信号；
S212：伺服控制器根据关节的碰撞信号，确认设备发生碰撞；
S2131：伺服控制器生成停机指令，发送给其他所有非碰撞关节的伺服驱动器；
S321：所有非碰撞关节的伺服驱动器接收停机指令；
S3221：所有非碰撞关节的伺服驱动器停止向关节伺服电机输出电流；
S3222：所有非碰撞关节的伺服驱动器停止向抱闸器输出电流；
S2132：伺服控制器发送碰撞报告给上位机；
S501：所有非碰撞关节的抱闸器失电抱闸；
S421：所有非碰撞关节的关节伺服电机失电停转；
S111：上位机接收碰撞报告；
S112：上位机发出碰撞警告。
参照图9，在一些实施例中，运动控制系统执行以下步骤：
S101：上位机接收用户输入的运动控制指令；
S102：上位机发送运动控制指令给伺服控制器；
S201：伺服控制器接收上位机发送的运动控制指令；
S202：伺服控制器解析运动控制指令，确定运动控制任务；
S203：伺服控制器根据运动控制任务，计算各关节的关节理论运动状态；
S204：伺服控制器根据各关节的关节理论运动状态，计算各关节伺服电机的理论力矩和电机理论运动状态，以及确定调节力矩阈值；
S205：伺服控制器发送各关节伺服电机的电机理论运动状态、理论 力矩、及调节力矩阈值给各关节伺服驱动器；
S301：伺服驱动器接收伺服控制器发送的该关节伺服电机的电机理论运动状态、理论力矩及调节力矩阈值；
S302：伺服驱动器根据关节伺服电机的理论力矩和调节力矩阈值计算电流限制范围；
S303：伺服驱动器将电机理论运动状态作为PID控制模块的输入，计算关节伺服电机的驱动电流，并根据电流限制范围对驱动电流进行限制；
S401：关节伺服电机接收驱动电流，驱动电机转子转动，编码器检测电机转子的实际转角，发送电机转子的实际转角给伺服驱动器；
S411：关节伺服电机发送异常的电机转子的实际转角给伺服驱动器；
S311：伺服驱动器接收异常的电机转子的实际转角，发送给伺服控制器；
S211：伺服控制器接收异常的电机转子的实际转角；
S212：伺服控制器将异常的电机转子的实际转角与理论运动状态中电机转子的理论转角进行比较，计算跟踪误差；
S213：伺服控制器比较跟踪误差是否大于碰撞阈值；
S214：若比较结果为是，则伺服控制器判定发生碰撞；
S2151：伺服控制器发送停机指令给关节的伺服驱动器；
S321：所有的伺服驱动器接收停机指令；
S3221：所有的伺服驱动器停止向关节伺服电机输出电流；
S421：所有关节伺服电机失电停转；
S3222：所有的伺服驱动器停止向抱闸器输出电流；
S501：所有的关节的抱闸器失电抱闸；
S2152：伺服控制器发送碰撞报告给上位机；
S111：上位机接收碰撞报告；
S112：上位机发出碰撞警告。
综上，本申请的技术方案，在控制协作机械臂运动的过程中，根据协作机械臂的理论运动状态来计算电流限制范围，并控制协作机械臂的各个关节伺服电机的驱动电流不超出该电流限制范围；利用关节理论运动状态以及各个关节伺服电机的理论力矩直接对各个关节的驱动电流进行限制，有效控制各个关节对外的输出力矩大小，减少协作机械臂在发生碰撞时对碰撞的障碍物或人产生的伤害，有效保护协作机械臂和人。
如下表格中展示了示例性的示出了某协作机械臂以及工业安全的相关参数。
Figure PCTCN2022138465-appb-000028
Figure PCTCN2022138465-appb-000029
根据上述图表数据结合上述实施例容易看出，通过对协作机械臂的各个关节的驱动电流进行限制，能够限制碰撞时关节的输出力矩增大区间，进而使协作机械臂在发生碰撞时，对碰撞物造成的最大的碰撞夹伤压力也仅在200N，远小于一般工况下协作机械臂输出最大输出力矩时，产生的1000N碰撞夹伤压力，且低于人体能承受的最大安全碰撞压力210N，能有效提高协作机械臂工作时的安全性。应当理解的是，本申请技术方案主要追求的是降低碰撞发生时对人体造成的伤害，无非完全避免伤害发生。
根据调节力矩阈值来设置电流限制范围的电流输出约束上限和电流输出约束下限，调节力矩阈值可以是根据用户通过上位机发送，从而便捷的根据用户的需求设置电流限制范围。
调节力矩阈值也可以是根据当前环境或应用场景的安全档位相关，可以根据不同的安全需求灵活的设置电流限制范围的大小，能满足协作机械臂在不同的使用环境或应用场景下的不同需求。
下面以运动控制系统中各个关节的伺服驱动器作为执行主体继续阐述图8所示的碰撞检测方法：
请继续参阅图13，图13是本申请提供的碰撞检测方法第三实施例的流程示意图。在本实施例的碰撞检测方法具体应用于一种伺服驱动器，如图5所示运动控制系统中的任意一个关节的伺服驱动器。
如图13所示，在本实施例的碰撞检测方法具体包括以下步骤：
步骤S61：接收关节伺服电机的电机理论运动状态和理论力矩。
步骤S62：根据理论力矩，计算电流限制范围。
步骤S63：控制输出给伺关节服电机的驱动电流不超出电流限制范围。
步骤S64：接收关节伺服电机的实际运动状态。
在本实施例中，请参阅图3中的S311，伺服驱动器获取关节伺服电机的编码器检测的电机转子的实际转角，利用电机转子的实时位置确定关节伺服电机的实际运动状态。
步骤S65：根据关节伺服电机的电机理论运动状态和实际运动状态，计算跟踪误差。
步骤S66：当跟踪误差超过碰撞阈值时，判定所在关节发生碰撞。
基于上述运动控制方法和碰撞检测方法的实施例，本申请还提出了一种协作机械臂，具体请参阅图14，图14是本申请提供的协作机械臂一实施例的硬件结构示意图。
本实施例的协作机械臂700包括处理器71、存储器72、输入输出设备73以及总线74。
该处理器71、存储器72、输入输出设备73分别与总线74相连，该存储器72中存储有程序数据，处理器71用于执行程序数据以实现上述实施例的运动控制方法和/或碰撞检测方法。
在本实施例中，处理器71还可以称为CPU(Central Processing Unit，中央处理单元)。处理器71可能是一种集成电路芯片，具有信号的处理能力。处理器71还可以是通用处理器、数字信号处理器(DSP，Digital Signal Process)、专用集成电路(ASIC，Application Specific Integrated Circuit)、现场可编程门阵列(FPGA，Field Programmable Gate Array)或者其它可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。通用处理器可以是微处理器或者该处理器71也可以是任何常规的处理器等。
基于上述运动控制方法和碰撞检测方法的实施例，本申请还提出了一种工业机器人，具体请参阅图15，图15是本申请提供的工业机器人一实施例的硬件结构示意图。
本实施例的工业机器人800包括处理器81、存储器82、输入输出设备83以及总线84。
该处理器81、存储器82、输入输出设备83分别与总线84相连，该存储器82中存储有程序数据，处理器81用于执行程序数据以实现上述实施例的运动控制方法和/或碰撞检测方法。
本申请还提供一种计算机存储介质，请继续参阅图16，图16是本申请提供的计算机存储介质一实施例的结构示意图，该计算机存储介质900中存储有程序数据91，该程序数据91在被处理器执行时，用以实现上述实施例的运动控制方法/或碰撞检测方法。
本申请的实施例以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)或处理器(processor)执行本申请各个实施方式所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各 种可以存储程序代码的介质。
以上所述仅为本申请的实施方式，并非因此限制本申请的专利范围，方式利用本申请说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本申请的专利保护范围内。

Claims (45)

  1. 一种协作机械臂的运动控制方法，其特征在于，包括：
    根据运动控制任务，计算关节的关节理论运动状态；
    根据所述关节理论运动状态，计算关节伺服电机的理论力矩；
    根据所述理论力矩，计算所述关节伺服电机的电流限制范围；
    控制输出给所述关节伺服电机的驱动电流不超出所述电流限制范围。
  2. 根据权利要求1所述的运动控制方法，其特征在于，
    所述根据所述理论力矩，计算所述关节伺服电机的电流限制范围，包括：
    根据所述理论力矩以及所述关节伺服电机的调节力矩阈值，计算所述电流限制范围。
  3. 根据权利要求2所述的运动控制方法，其特征在于，
    所述电流限制范围包括电流输出约束上限和电流输出约束下限；
    所述控制输出给所述关节伺服电机的驱动电流不超出所述电流限制范围，包括：
    当所述输出给所述关节伺服电机的驱动电流超过所述电流输出约束上限时，按照所述电流输出约束上限对所述驱动电流进行调整，得到调整后的驱动电流；
    当所述输出给所述关节伺服电机的驱动电流低于所述电流输出约束下限时，按照所述电流输出约束下限对所述驱动电流进行调整，得到调整后的驱动电流。
  4. 根据权利要求3所述的运动控制方法，其特征在于，所述方法还包括：
    根据所述理论力矩与所述调节力矩阈值确定力矩约束上限和力矩约束下限；
    基于所述力矩约束上限与预设的转矩常数计算所述电流输出约束上限；
    基于所述力矩约束下限与所述预设的转矩常数计算所述电流输出约束下限。
  5. 根据权利要求1所述的运动控制方法，其特征在于，
    所述根据运动控制任务，计算关节的关节理论运动状态之后，所述运动控制方法还包括：
    根据所述关节理论运动状态计算所述关节伺服电机的电机理论运动状态；
    将所述电机理论运动状态输入对应的比例微积分控制模块，计算所 述关节伺服电机的驱动电流。
  6. 根据权利要求5所述的运动控制方法，其特征在于，
    所述电机理论运动状态包括理论转角状态，所述比例微积分控制模块为位置式闭环比例微积分控制模块。
  7. 根据权利要求6所述的运动控制方法，其特征在于，
    所述控制输出给所述关节伺服电机的驱动电流不超出所述电流限制范围，包括：
    获取所述关节伺服电机的实际转角；
    根据所述电机理论运动状态，获取当前时刻对应的所述关节伺服电机的理论转角；
    将所述实际转角和所述理论转角输入到所述比例微积分控制模块或将所述实际转角与所述理论转角的差值输入到所述比例微积分控制模块，通过所述比例微积分控制模块对输出给所述关节伺服电机的驱动电流进行调整，控制调整后的驱动电流不超出所述电流限制范围。
  8. 根据权利要求1所述的运动控制方法，其特征在于，所述根据所述关节理论运动状态，计算关节伺服电机的理论力矩，包括；
    将所述关节理论运动状态输入预设的逆动力学模型；
    利用所述逆动力学模型对所述关节理论运动状态进行运算，得到关节理论力矩；
    获取所述关节的减速比；
    根据所述关节理论力矩和所述减速比，计算所述关节伺服电机的理论力矩；
    其中，所述关节理论运动状态包括关节转角、关节速度以及关节加速度。
  9. 根据权利要求8所述的运动控制方法，其特征在于，
    所述利用所述逆动力学模型对所述关节理论运动状态进行运算，得到关节理论力矩，包括：
    利用所述逆动力学模型中的机器人惯量和加速度，获取惯量力矩；
    利用所述逆动力学模型中的科氏力与离心力模型和速度，获取科氏力与离心力力矩；
    利用所述逆动力学模型中的重力模型和关节转角，获取重力力矩；
    利用所述逆动力学模型中的摩擦力模型和关节速度，获取摩擦力力矩；
    将所述惯量力矩、所述科氏力与离心力力矩、所述重力力矩以及所述摩擦力力矩相加，得到所述关节理论力矩。
  10. 根据权利要求9所述的运动控制方法，其特征在于，
    所述利用所述逆动力学模型中的摩擦力模型和关节速度，获取摩擦力力矩，包括：
    利用所述摩擦力模型中的粘性摩擦函数和库伦摩擦函数对所述关节速度进行处理，得到所述摩擦力力矩。
  11. 根据权利要求1所述的运动控制方法，其特征在于，
    所述根据运动控制任务，计算关节的关节理论运动状态之前，所述运动控制方法还包括：
    接收运动控制指令，解析所述运动控制指令，确定所述运动控制任务。
  12. 一种协作机械臂的运动控制方法，其特征在于，所述运动控制方法应用于伺服控制器，包括：
    根据所述运动控制任务，计算关节的关节理论运动状态；
    根据所述关节理论运动状态，计算关节伺服电机的理论力矩；
    将所述理论力矩发送给伺服驱动器；
    其中，所述理论力矩用于供所述伺服驱动器计算所述关节伺服电机的电流限制范围，并控制输出给所述关节伺服电机的驱动电流不超出所述电流限制范围。
  13. 根据权利要求12所述的运动控制方法，其特征在于，所述将所述理论力矩发送给伺服驱动器，包括：
    将所述理论力矩和所述关节伺服电机的调节力矩阈值发送给所述伺服驱动器，用于供所述伺服驱动器计算所述电流限制范围，并控制输出给所述关节伺服电机的驱动电流不超出所述电流限制范围。
  14. 根据权利要求12所述的运动控制方法，其特征在于，所述根据所述关节理论运动状态，计算关节伺服电机的理论力矩，包括：
    将所述关节理论运动状态输入预设的逆动力学模型；
    利用所述逆动力学模型对所述关节理论运动状态进行运算，得到关节理论力矩；
    获取所述关节的减速比；
    根据所述关节理论力矩和所述减速比，计算所述关节伺服电机的理论力矩；
    其中，所述关节理论运动状态包括关节转角、关节速度以及关节加速度。
  15. 根据权利要求14所述的运动控制方法，其特征在于，所述利用所述逆动力学模型对所述关节理论运动状态进行运算，得到关节理论力矩，包括：
    利用所述逆动力学模型中的机器人惯量和加速度，获取惯量力矩；
    利用所述逆动力学模型中的科氏力与离心力模型和速度，获取科氏力与离心力力矩；
    利用所述逆动力学模型中的重力模型和关节转角，获取重力力矩；
    利用所述逆动力学模型中的摩擦力模型和关节速度，获取摩擦力力 矩；
    将所述惯量力矩向量、所述科氏力与离心力力矩向量、所述重力力矩向量以及所述摩擦力力矩相加，得到所述关节理论力矩。
  16. 根据权利要求15所述的运动控制方法，其特征在于，
    所述利用所述逆动力学模型中的摩擦力模型和关节速度，获取摩擦力力矩，包括：
    利用所述摩擦力模型中的粘性摩擦函数和库伦摩擦函数对所述关节速度进行处理，得到所述摩擦力力矩。
  17. 根据权利要求12所述的运动控制方法，其特征在于，
    所述根据所述运动控制任务，计算关节的关节理论运动状态之前，所述运动控制方法还包括：
    接收运动控制指令，解析所述运动控制指令，确定所述运动控制任务。
  18. 根据权利要求12所述的运动控制方法，其特征在于，还包括：
    根据所述关节理论运动状态，计算所述关节伺服电机的电机理论运动状态，并将所述电机理论运动状态发送给所述伺服驱动器；
    或者，将所述关节理论运动状态发送给所述伺服驱动器，以使所述伺服驱动器根据所述关节理论运动状态计算得到所述电机理论运动状态；
    其中，所述电机理论运动状态用于输入比例微积分控制模块以计算所述关节伺服电机的驱动电流。
  19. 一种协作机械臂的运动控制方法，其特征在于，所述运动控制方法应用于任意一个关节的伺服驱动器，所述运动控制方法包括：
    接收关节伺服电机的理论力矩；
    根据所述理论力矩，计算电流限制范围；
    控制输出给关节伺服电机的驱动电流不超出所述电流限制范围。
  20. 根据权利要求19所述的运动控制方法，其特征在于，
    所述根据所述理论力矩，计算电流限制范围，包括：
    根据所述理论力矩和调节力矩阈值，计算所述电流限制范围；
    其中，所述调节力矩阈值由伺服控制器发送。
  21. 根据权利要求20所述的运动控制方法，其特征在于，
    所述运动控制方法，还包括：
    接收所述关节伺服电机的电机理论运动状态，所述电机理论运动状态由所述伺服控制器根据关节理论运动状态计算得到；
    或者，接收所述关节的关节理论运动状态，根据所述关节理论运动状态计算电机理论运动状态；
    所述控制输出给关节伺服电机的驱动电流不超出所述电流限制范围，包括：
    将所述电机理论运动状态输入比例微积分控制模块，计算所述关节伺服电机的驱动电流；
    根据所述电流限制范围对所述驱动电流进行约束，以使输出给所述关节伺服电机的驱动电流不超出所述电流限制范围。
  22. 根据权利要求21所述的运动控制方法，其特征在于，所述运动控制方法还包括：
    获取所述关节伺服电机的实际转角；
    根据所述电机理论运动状态，获取当前时刻对应的所述关节伺服电机的理论转角；
    将所述实际转角和所述理论转角输入到所述比例微积分控制模块或将所述实际转角与所述理论转角的差值输入到所述比例微积分控制模块，通过所述比例微积分控制模块对输出给所述关节伺服电机的驱动电流进行调整，控制调整后的驱动电流不超出所述电流限制范围。
  23. 根据权利要求20所述的运动控制方法，其特征在于，
    所述电流限制范围包括电流输出约束上限和电流输出约束下限；
    所述控制输出给关节伺服电机的驱动电流不超出所述电流限制范围，包括：
    当所述给关节伺服电机的驱动电流超过所述电流输出约束上限时，按照所述电流输出约束上限对所述驱动电流进行调整，得到调整后的驱动电流；
    当所述给关节伺服电机的驱动电流低于所述电流输出约束下限时，按照所述电流输出约束下限对所述驱动电流进行调整，得到调整后的驱动电流。
  24. 根据权利要求23所述的运动控制方法，其特征在于，所述方法还包括：
    根据所述理论力矩与所述调节力矩阈值确定力矩约束上限和力矩约束下限；
    基于所述力矩约束上限与预设的转矩常数计算所述电流输出约束上限；
    基于所述力矩约束下限与所述预设的转矩常数计算所述电流输出约束下限。
  25. 一种协作机械臂的运动控制系统，包括上位机、伺服控制器、多个关节的伺服驱动器以及多个关节伺服电机；
    所述上位机与所述伺服控制器通信连接，所述伺服控制器分别与各个所述伺服驱动器通信连接，各个所述伺服驱动器用于驱动对应的关节伺服电机；
    其特征在于，所述协作机械臂的运动控制系统，在工作时配置有运动控制，所述运动控制执行如权利要求1-11任一项所述的运动控制方法。
  26. 一种协作机械臂的碰撞检测方法，其特征在于，所述碰撞检测方法包括：
    根据运动控制任务，计算关节的关节理论运动状态；
    根据所述关节理论运动状态，计算关节伺服电机的理论力矩和电机理论运动状态；
    根据所述理论力矩，计算所述关节伺服电机的电流限制范围；
    控制输出给所述关节伺服电机的驱动电流不超出所述电流限制范围；
    获取所述关节伺服电机的实际运动状态；
    根据所述实际运动状态和所述电机理论运动状态，计算跟踪误差；
    当所述跟踪误差超过碰撞阈值时，判定所述协作机械臂发生碰撞。
  27. 根据权利要求26所述的碰撞检测方法，其特征在于，
    所述判定所述协作机械臂发生碰撞之后，包括：
    停止向所述协作机械臂的各个关节伺服电机供电；和/或，
    停止向所述协作机械臂的各个关节的抱闸器供电。
  28. 根据权利要求26所述的碰撞检测方法，其特征在于，
    所述判定所述协作机械臂发生碰撞之后，所述碰撞检测方法还包括：
    控制所述协作机械臂的各个关节执行回退动作。
  29. 根据权利要求26所述的碰撞检测方法，其特征在于，
    所述根据所述实际运动状态和关节伺服电机的电机理论运动状态，计算跟踪误差，包括：
    基于所述关节伺服电机的实际运动状态，获取所述关节伺服电机的实际转角；
    基于所述关节伺服电机的电机理论运动状态，获取所述关节伺服电机的理论转角；
    根据所述实际转角和理论转角的差值，确定转角跟踪误差。
  30. 根据权利要求26所述的碰撞检测方法，其特征在于，
    所述根据所述实际运动状态和关节伺服电机的电机理论运动状态，计算跟踪误差，包括：
    基于所述关节伺服电机的实际运动状态，获取所述关节伺服电机的实际速度；
    基于所述关节伺服电机的电机理论运动状态，获取所述关节伺服电机的理论速度；
    根据所述实际速度和理论速度的差值，确定速度跟踪误差。
  31. 根据权利要求30所述的碰撞检测方法，其特征在于，
    所述实际速度根据所述关节的位置变化快慢计算。
  32. 根据权利要求26所述的碰撞检测方法，其特征在于，
    所述碰撞阈值根据所述关节的负载和/或所述关节的运动速度确定。
  33. 一种协作机械臂的碰撞检测方法，其特征在于，所述碰撞检测方法应用于一种伺服控制器，包括：
    根据运动控制任务，计算关节的关节理论运动状态；
    根据所述关节理论运动状态，计算关节伺服电机的理论力矩和电机理论运动状态；
    将所述理论力矩和所述电机理论运动状态发送给对应的伺服驱动器；
    获取所述关节伺服电机的实际运动状态；
    根据所述实际运动状态和所述电机理论运动状态，计算跟踪误差；
    当所述跟踪误差超过碰撞阈值时，判定所述协作机械臂发生碰撞，并发送停机指令给所述伺服驱动器；
    其中，所述理论力矩用于所述伺服驱动器计算所述关节伺服电机的电流限制范围，并控制输出给所述关节伺服电机的驱动电流不超出所述电流限制范围。
  34. 根据权利要求33所述的碰撞检测方法，其特征在于，还包括：
    当判定所述协作机械臂发生碰撞时，发送碰撞报告给上位机，以使所述上位机根据所述碰撞报告发出碰撞警告。
  35. 根据权利要求33所述的碰撞检测方法，其特征在于，
    所述根据所述实际运动状态和关节伺服电机的电机理论运动状态，计算各个关节的跟踪误差，包括：
    基于所述关节伺服电机的实际运动状态，获取所述关节伺服电机的实际转角；
    基于所述关节伺服电机的电机理论运动状态，获取关节伺服电机的理论转角；
    根据所述实际转角和所述理论转角的差值，确定转角跟踪误差。
  36. 根据权利要求33所述的碰撞检测方法，其特征在于，
    所述根据所述实际运动状态和所述关节伺服电机的电机理论运动状态，计算跟踪误差，包括：
    基于所述关节伺服电机的实际运动状态，获取所述关节伺服电机的实际速度；
    基于所述关节伺服电机的电机理论运动状态，获取所述关节伺服电机的理论速度；
    根据所述实际速度和所述理论速度的差值，确定速度跟踪误差。
  37. 一种协作机械臂的碰撞检测方法，其特征在于，所述碰撞检测方法应用于任意一个关节的伺服驱动器，所述碰撞检测方法包括：
    接收关节伺服电机的理论力矩和电机理论运动状态；
    根据所述理论力矩，计算电流限制范围；
    控制输出给所述关节伺服电机的驱动电流不超出所述电流限制范 围；
    接收所述关节伺服电机的实际运动状态；
    根据所述关节伺服电机的电机理论运动状态和实际运动状态，计算跟踪误差；
    当所述跟踪误差超过碰撞阈值时，判定所在关节发生碰撞。
  38. 根据权利要求37所述的碰撞检测方法，其特征在于，
    所述判定所在关节发生碰撞之后，所述碰撞检测方法还包括：
    停止向所在关节伺服电机供电；和/或，
    停止向所在关节的抱闸器供电。
  39. 根据权利要求37所述的碰撞检测方法，其特征在于，
    所述判定所在关节发生碰撞之后，所述碰撞检测方法还包括：
    发送关节的碰撞信号给伺服控制器，以使所述伺服控制器根据所述碰撞信号向其他关节的伺服驱动器发送执行回退指令和/或停止指令。
  40. 根据权利要求36所述的碰撞检测方法，其特征在于，
    所述根据所述关节伺服电机的电机理论运动状态和实际运动状态，计算跟踪误差，包括：
    基于所述关节伺服电机的实际运动状态，获取所述关节伺服电机的实际转角；
    基于关节伺服电机的电机理论运动状态，获取所述关节伺服电机的理论转角；
    根据所述实际转角和所述理论转角的差值，确定转角跟踪误差。
  41. 根据权利要求36所述的碰撞检测方法，其特征在于，
    所述根据所述关节伺服电机的电机理论运动状态和实际运动状态，计算跟踪误差，包括：
    基于所述关节伺服电机的实际运动状态，获取所述关节伺服电机的实际速度；
    基于所述关节伺服电机的电机理论运动状态，获取所述关节伺服电机的理论速度；
    根据所述实际速度和所述理论速度的差值，确定速度跟踪误差。
  42. 一种协作机械臂的控制系统，包括上位机、伺服控制器、多个关节的伺服驱动器以及多个关节伺服电机；
    所述上位机与所述伺服控制器通信连接，所述伺服控制器分别与各个所述关节的伺服驱动器通信连接，各个所述关节的伺服驱动器用于驱动对应的关节伺服电机；
    其特征在于：
    所述协作机械臂的控制系统，在工作时配置有碰撞检测，所述碰撞检测执行如权利要求26-32任一项所述的碰撞检测方法。
  43. 一种协作机械臂，其特征在于，所述协作机械臂包括处理器以及 与所述处理器耦接的存储器；
    其中，所述存储器用于存储程序数据，所述处理器用于执行所述程序数据以实现如权利要求1-11、11-18、19-24任一项所述的运动控制方法和/或权利要求26-32、33-36、37-41任一项所述的碰撞检测方法。
  44. 一种工业机器人，其特征在于，所述工业机器人包括处理器以及与所述处理器耦接的存储器；
    其中，所述存储器用于存储程序数据，所述处理器用于执行所述程序数据以实现如权利要求1-11、11-18、19-24任一项所述的运动控制方法和/或权利要求26-32、33-36、37-41任一项所述的碰撞检测方法。
  45. 一种计算机存储介质，其特征在于，所述计算机存储介质用于存储程序数据，所述程序数据在被处理器执行时，用以实现如权利要求1-11、11-18、19-24任一项所述的运动控制方法和/或权利要求26-32、33-36、37-41任一项所述的碰撞检测方法。
PCT/CN2022/138465 2022-09-30 2022-12-12 协作机械臂及其运动控制方法、碰撞检测方法、控制系统 WO2024066063A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202211220131.1A CN115674190B (zh) 2022-09-30 2022-09-30 协作机械臂及其运动控制方法、碰撞检测方法、控制系统
CN202211220131.1 2022-09-30

Publications (1)

Publication Number Publication Date
WO2024066063A1 true WO2024066063A1 (zh) 2024-04-04

Family

ID=85064152

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2022/138465 WO2024066063A1 (zh) 2022-09-30 2022-12-12 协作机械臂及其运动控制方法、碰撞检测方法、控制系统

Country Status (2)

Country Link
CN (1) CN115674190B (zh)
WO (1) WO2024066063A1 (zh)

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2000052286A (ja) * 1998-07-31 2000-02-22 Yaskawa Electric Corp ロボットの制御装置
JP2002283276A (ja) * 2001-03-21 2002-10-03 Daihen Corp 多関節ロボットにおける衝突検出・停止制御法
CN107253196A (zh) * 2017-08-01 2017-10-17 中科新松有限公司 一种机械臂碰撞检测方法、装置、设备及存储介质
CN108247654A (zh) * 2018-02-05 2018-07-06 遨博（北京）智能科技有限公司 一种机器人的控制方法、装置和系统
CN111906775A (zh) * 2020-06-08 2020-11-10 深圳众为兴技术股份有限公司 一种机器人碰撞检测方法、装置、存储介质及机器人
CN112894821A (zh) * 2021-01-30 2021-06-04 同济大学 基于电流法的协作机器人拖动示教控制方法、装置及设备
CN112936260A (zh) * 2021-01-26 2021-06-11 华南理工大学 一种六轴工业机器人无传感器碰撞检测方法及系统
CN113021353A (zh) * 2021-03-26 2021-06-25 福州大学 机器人碰撞检测方法
CN113319853A (zh) * 2021-06-18 2021-08-31 广东智源机器人科技有限公司 机器人控制方法、装置、电子设备以及机器人系统
CN113681567A (zh) * 2021-09-14 2021-11-23 北京清飞科技有限公司 一种新型无传感器机器人碰撞检测方法及其系统

Family Cites Families (32)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3152056B2 (ja) * 1994-03-23 2001-04-03 三菱電機株式会社 産業ロボットの制御方法
JP2012055981A (ja) * 2010-09-06 2012-03-22 Denso Wave Inc ロボット制御装置およびロボット制御方法
KR101844542B1 (ko) * 2016-03-04 2018-04-02 주식회사 사이보그-랩 협업로봇의 충돌을 감지하는 장치 및 방법
CN208614800U (zh) * 2018-02-05 2019-03-19 遨博（北京）智能科技有限公司 一种机器人的控制系统
CN108789408A (zh) * 2018-06-20 2018-11-13 上海交通大学 基于力矩传感器的协作机器人驱控一体化控制系统
CN109531565B (zh) * 2018-08-27 2023-11-03 苏州博众智能机器人有限公司 一种机械臂的控制方法、装置、服务机器人及存储介质
JP2020044610A (ja) * 2018-09-19 2020-03-26 株式会社デンソーウェーブ ロボットの制御方法
CN109079800A (zh) * 2018-10-29 2018-12-25 珠海格力智能装备有限公司 机器人碰撞检测方法、装置、存储介质、处理器及机器人
WO2020133270A1 (zh) * 2018-12-28 2020-07-02 深圳配天智能技术研究院有限公司 机器人的动力学参数辨识方法、机器人和存储装置
CN109732599B (zh) * 2018-12-29 2020-11-03 深圳市越疆科技有限公司 一种机器人碰撞检测方法、装置、存储介质及机器人
CN109807899B (zh) * 2019-03-08 2020-09-29 哈工大机器人（合肥）国际创新研究院 针对拖动示教的协作机器人摩擦力矩补偿方法
CN110000815B (zh) * 2019-04-09 2022-03-01 达闼机器人有限公司 一种碰撞检测方法、装置、电子设备及存储介质
CN110026981B (zh) * 2019-04-19 2021-09-17 中科新松有限公司 一种基于模型自适应的机械臂碰撞检测方法
CN110787026B (zh) * 2019-11-11 2022-04-12 上海电气集团股份有限公司 一种基于多传感器信息的运动异常保护方法及康复设备
CN110815233B (zh) * 2019-11-28 2020-12-08 北京云迹科技有限公司 机器人驱动方法、装置、存储介质及机器人
CN111152231A (zh) * 2020-01-07 2020-05-15 成都卡诺普自动化控制技术有限公司 一种基于力控的机器人示教方法
CN111230869B (zh) * 2020-01-21 2021-07-13 北京卫星制造厂有限公司 一种复杂空间曲线焊缝运动轨迹和焊接工艺协同规划方法
DE102020103857B4 (de) * 2020-02-14 2023-03-02 Franka Emika Gmbh Kraftbegrenzung bei Kollision eines Robotermanipulators
CN111923045B (zh) * 2020-08-07 2021-10-29 珠海格力智能装备有限公司 机器人的控制方法、装置、计算机可读存储介质和处理器
CN112123333A (zh) * 2020-08-18 2020-12-25 珠海格力电器股份有限公司 一种多关节机器人碰撞检测方法及其装置
CN214724190U (zh) * 2020-11-18 2021-11-16 江汉大学 一种自动化机器人控制系统
CN114603599A (zh) * 2020-12-08 2022-06-10 山东新松工业软件研究院股份有限公司 一种机器人碰撞检测方法、装置、计算机设备及存储介质
WO2022161245A1 (zh) * 2021-01-29 2022-08-04 苏州艾利特机器人有限公司 一种提高机器人关节转矩检测精度的方法及多关节机器人
CN114952821A (zh) * 2021-02-25 2022-08-30 腾讯科技（深圳）有限公司 机器人运动控制方法、机器人及系统
CN113752250A (zh) * 2021-05-31 2021-12-07 腾讯科技（深圳）有限公司 机器人关节的控制方法、装置、机器人及存储介质
CN215825344U (zh) * 2021-09-29 2022-02-15 宁波睿达医疗器械有限公司 机械臂
CN113664869A (zh) * 2021-09-29 2021-11-19 宁波睿达医疗器械有限公司 机械臂、机械臂控制方法及系统
CN113771047B (zh) * 2021-10-29 2023-02-21 遨博（北京）智能科技有限公司 对机械臂的碰撞处理方法、控制柜、以及机械臂系统
CN114161477A (zh) * 2021-12-03 2022-03-11 厦门驱动未来科技有限公司 一种工业机器人碰撞检测方法
CN114734437A (zh) * 2022-03-25 2022-07-12 中国科学院自动化研究所 机器人关节控制方法及装置
CN114475244B (zh) * 2022-04-14 2022-09-23 禾多科技（北京）有限公司 定速巡航控制方法、整车控制器及定速巡航控制系统
CN114604139B (zh) * 2022-04-21 2024-04-12 福建中维动力科技股份有限公司 电池电流控制方法及终端

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2000052286A (ja) * 1998-07-31 2000-02-22 Yaskawa Electric Corp ロボットの制御装置
JP2002283276A (ja) * 2001-03-21 2002-10-03 Daihen Corp 多関節ロボットにおける衝突検出・停止制御法
CN107253196A (zh) * 2017-08-01 2017-10-17 中科新松有限公司 一种机械臂碰撞检测方法、装置、设备及存储介质
CN108247654A (zh) * 2018-02-05 2018-07-06 遨博（北京）智能科技有限公司 一种机器人的控制方法、装置和系统
CN111906775A (zh) * 2020-06-08 2020-11-10 深圳众为兴技术股份有限公司 一种机器人碰撞检测方法、装置、存储介质及机器人
CN112936260A (zh) * 2021-01-26 2021-06-11 华南理工大学 一种六轴工业机器人无传感器碰撞检测方法及系统
CN112894821A (zh) * 2021-01-30 2021-06-04 同济大学 基于电流法的协作机器人拖动示教控制方法、装置及设备
CN113021353A (zh) * 2021-03-26 2021-06-25 福州大学 机器人碰撞检测方法
CN113319853A (zh) * 2021-06-18 2021-08-31 广东智源机器人科技有限公司 机器人控制方法、装置、电子设备以及机器人系统
CN113681567A (zh) * 2021-09-14 2021-11-23 北京清飞科技有限公司 一种新型无传感器机器人碰撞检测方法及其系统

Also Published As

Publication number Publication date
CN115674190A (zh) 2023-02-03
CN115674190B (zh) 2024-05-07

Similar Documents

Publication Publication Date Title
EP2372478B1 (en) Motor driving system and motor controller
WO2017094240A1 (ja) ロボットシステムの監視装置
US9821459B2 (en) Multi-joint robot having function for repositioning arm
JP2608161B2 (ja) 産業用ロボットの停止制御方法
KR101265903B1 (ko) 머니퓰레이터를 정지시키기 위한 방법 및 장치
JP3212571B2 (ja) 産業用ロボット
US9409295B2 (en) Method for controlling a robot
EP2939797A2 (en) Motion limiting device and motion limiting method
EP3723944A1 (en) Robot comprising safety system ensuring stopping time and distance
JP2010137312A (ja) ロボットシステムおよび制御方法
CN109746942B (zh) 机器人、运动控制系统及机器人防碰撞方法
WO2019194138A1 (ja) ロボットの制御装置
KR102418451B1 (ko) 로봇 제어 시스템
WO1991010181A1 (en) Method of detecting collision using observer
CN109311164B (zh) 对机器人组的监视
WO2024066063A1 (zh) 协作机械臂及其运动控制方法、碰撞检测方法、控制系统
Je et al. Current based compliance control method for minimizing an impact force at collision of service robot arm
JP2011152612A (ja) ロボット制御装置
JPH01310889A (ja) 産業用ロボットの制御装置
JPS63308607A (ja) 産業用ロボットの制御装置
JP2017159428A (ja) 制御装置、ロボット、及びロボットシステム
JP2001353687A (ja) ロボットの制御装置とその制御方法
JP2002144277A (ja) ロボット
JP2012110971A (ja) ロボット
JPH06170754A (ja) マスタ・スレーブロボットの制御装置

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 22960658

Country of ref document: EP

Kind code of ref document: A1