Method, device and storage medium for controlling a multi-axis motion machine
By receiving task-level instructions in a multi-axis motion machine and decomposing it into environmental monitoring, motion control and mode switching subtasks, the autonomous switching of multiple motion control modes is achieved, solving the shortcomings of manual operation and manual programming switching in the prior art. , improves operational efficiency and accuracy.
Patent Information
- Application Number
- CN201980100024.5
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2019-09-29
- Publication Date
- 2024-11-19
- Estimated Expiration
- 2039-09-29
AI Technical Summary
The existing multi-axis motion machines switch between different motion control modes require manual operation or manual programming, and cannot switch independently according to task-level instructions, and cannot meet the needs of flexible manufacturing.
By receiving task-level instructions, it is decomposed into environmental monitoring subtasks, motion control subtasks and motion control mode switching subtasks. Based on environmental information, it plans the motion trajectory and switch control modes in real time, including position control, Cartesian space impedance control, and axis system space Impedance control and gravity compensation mode.
The multi-axis motion machine is automatically switched in multiple motion control modes, improving operational efficiency and accuracy, able to move to the target position quickly, avoid hitting targets and non-target objects, and adapt to a variety of task-level instructions.
Smart Images

Figure CN114341823B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to a method, device and computer-readable storage medium for controlling a multi-axis motion machine, and in particular to a method, device and computer-readable storage medium capable of controlling a multi-axis motion machine to autonomously switch between multiple motion control modes according to task-level instructions. Background Art
[0002] Multi-axis motion machines, such as multi-axis manipulators and collaborative robots, are widely used in modern industrial production to pick up and deliver workpieces, process workpieces, or collaborate with operators to complete designated tasks. In multi-axis motion machines, the multi-axis motion controller used to control the multi-axis motion of the multi-axis motion machine is the core component. When performing different types of tasks, for safety reasons and the requirements of the application environment, multi-axis motion machines usually need to work in different motion control modes (such as position control mode, impedance control mode, etc.). For example, when it is necessary to move quickly to a specified position, the multi-axis machine usually works in position control mode; when it is necessary to output an ideal amount of force, the multi-axis machine usually works in impedance control mode.
[0003] Existing multi-axis motion machines can usually only switch between two motion control modes through manual operation or manual programming. For the two switching methods of manual operation and manual programming, specific application scenarios need to be considered and programmers / operators are required to have professional knowledge in related fields. With the development of flexible manufacturing, this method of switching the motion control mode of multi-axis motion machines can no longer meet the requirements. Multi-axis motion machines that do not require manual operation and program modification and can autonomously switch between multiple motion control modes according to task-level instructions are undoubtedly more ideal. Summary of the invention
[0004] The present invention aims at the above problems existing in the prior art and provides a control method for a multi-axis motion machine that can autonomously switch between multiple motion control modes according to task-level instructions. The present invention also provides a multi-axis motion control device for controlling a multi-axis motion machine, wherein the multi-axis motion machine includes an end effector that can move around multiple axes. The present invention also provides a computer-readable storage medium containing the control method.
[0005] In one aspect, the present invention provides a multi-axis motion control method for controlling a multi-axis motion machine, the multi-axis motion machine comprising an end effector capable of moving around a plurality of axes, the method comprising:
[0006] receiving task-level instructions;
[0007] decomposing the task-level instruction into a plurality of subtasks, the plurality of subtasks including an environment monitoring subtask, in which environmental information around the multi-axis motion machine is sensed and determined;
[0008] Planning a motion trajectory, planning the motion trajectory of the end effector according to the static environment information obtained in the environment monitoring subtask;
[0009] The multiple subtasks also include:
[0010] A motion control subtask, determining in real time a motion control mode for controlling the end effector according to the dynamic environment information obtained in the environment monitoring subtask;
[0011] The motion control mode switching subtask switches the current motion control mode to the motion control mode determined in the motion control subtask and adjusts the corresponding control parameters.
[0012] The above multi-axis motion control method can autonomously switch between multiple motion control modes according to task-level instructions.
[0013] According to a preferred embodiment of the present invention, the initial motion control mode for controlling the end effector is a position control mode. The beneficial technical effect of this preferred embodiment is at least that the end effector can move quickly to the target position.
[0014] According to a preferred embodiment of the present invention, the task-level instruction includes one or more of the following items: moving the target object, processing the target object, interacting with the target object and avoiding hitting the target object, and avoiding hitting non-target objects. The beneficial technical effect of this preferred embodiment is at least that the control method can receive and implement multiple task-level instructions.
[0015] According to a preferred embodiment of the present invention, when the task-level instruction includes moving a target object, the environment monitoring subtask includes identifying the target object, calculating the current position and the target position of the target object. The beneficial technical effect of this preferred embodiment is at least that the control method can more accurately control the end effector according to the output of the environment monitoring subtask.
[0016] According to a preferred embodiment of the present invention, the motion control subtask includes determining that the motion control mode for controlling the end effector is a position control mode. The beneficial technical effect of this preferred embodiment is at least that the end effector can move quickly to a target position.
[0017] According to a preferred embodiment of the present invention, when the task-level instruction includes processing a target object, the environmental monitoring subtask includes identifying the target object, determining the part of the target object to be processed, calculating the position and orientation of the part to be processed, and calculating the current position and orientation of the end effector. The beneficial technical effect of this preferred embodiment is at least that the control method can process the part of the target object to be processed more accurately according to the output of the environmental monitoring subtask.
[0018] According to a preferred embodiment of the present invention, the motion control subtask includes switching the motion control mode for controlling the end effector to a Cartesian space impedance control mode according to the current position of the end effector, and the motion control mode switching subtask includes adjusting the control parameters corresponding to the Cartesian space impedance control mode according to the following formula:
[0019]
[0020] Where, τ is the output torque of the power system; g(q) is the gravity torque vector function; is the joint friction function; J(q) is the Jacobian matrix; Λ(x) is the inertia matrix; is the nonlinear term in Cartesian space; D d is the Cartesian space damping; K d is the Cartesian space stiffness; F c is the force applied by the end effector; K f To determine F c The force selection matrix in the direction of ; q is the joint position; is the joint velocity; x is the actual position of the end effector in Cartesian space; x d is the planned position of the end effector in Cartesian space; is the planned velocity of the end effector in Cartesian space; is the planned acceleration of the end effector in Cartesian space; is the position error of the end effector in Cartesian space. The beneficial technical effect of this preferred embodiment is at least that the control method can adjust the control parameters according to the formula to process the to-be-processed part of the target object more efficiently.
[0021] According to a preferred embodiment of the present invention, when the task-level instruction includes interacting with a target object and avoiding hitting the target object, the environment monitoring subtask includes identifying the target object, calculating the position of the target object, calculating the current position of the end effector, and calculating the interaction position between the end effector and the target object. The beneficial technical effect of this preferred embodiment is at least that the end effector can avoid hitting the target object while moving quickly to the target position according to the output of the environment monitoring subtask.
[0022] According to a preferred embodiment of the present invention, the motion control subtask includes switching the motion control mode for controlling the end effector to the axis space impedance control mode according to the current position of the end effector, and the motion control mode switching subtask includes adjusting the control parameters corresponding to the axis space impedance control mode according to the following formula:
[0023]
[0024] Where, τ is the output torque of the power system; M(q) is the inertia matrix; is the Coriolis / centrifugal matrix; g(q) is the gravitational torque vector function; is the joint friction function; k p and k d are the joint stiffness matrix and joint damping matrix, respectively, which can be configured manually; q is the actual joint position; is the actual joint velocity; q * is the ideal joint position; is the ideal joint speed; The beneficial technical effect of this preferred embodiment is at least that the control method can adjust the control parameters according to the formula to better achieve the purpose of interacting with the target object and avoiding hitting the target object.
[0025] According to a preferred embodiment of the present invention, when the task-level instruction includes avoiding collision with non-target objects, the environment monitoring subtask includes real-time monitoring whether there are non-target objects within a predetermined range around the multi-axis motion machine (or the end effector). The beneficial technical effect of this preferred embodiment is at least that the end effector can enter the required control mode in advance according to the output of the environment monitoring subtask.
[0026] According to a preferred embodiment of the present invention, the motion control subtask includes switching the motion control mode for controlling the end effector to a gravity compensation mode, and the motion control mode switching subtask includes adjusting the control parameters corresponding to the gravity compensation mode according to the following formula:
[0027]
[0028] Where, τ is the output torque of the power system; g(q) is the gravity torque vector function; is the joint friction function; q is the joint position; The beneficial technical effect of this preferred embodiment is at least that the control method can adjust the control parameters according to the formula to better achieve the purpose of avoiding hitting non-target objects.
[0029] On the other hand, the present invention also provides a multi-axis motion control device for controlling a multi-axis motion machine, wherein the multi-axis motion machine includes an end effector capable of moving around multiple axes, and the device includes the following modules:
[0030] An environmental monitoring module, used to sense and determine environmental information around the multi-axis motion machine;
[0031] A task planning module, used to decompose the received task-level instruction into multiple subtasks and plan the motion trajectory of the end effector according to the static environment information obtained by the environment monitoring module;
[0032] A motion control module, used for determining in real time a motion control mode for controlling the end effector according to the dynamic environment information obtained in the environment monitoring module, and for executing the motion control mode for controlling the end effector;
[0033] The motion control mode switching module is used to switch the current motion control mode of the end effector to the determined motion control mode and adjust the corresponding control parameters.
[0034] The multi-axis motion control device can autonomously switch between multiple motion control modes according to task-level instructions.
[0035] According to a preferred embodiment of the present invention, the motion control module includes an impedance control submodule for executing an impedance control mode, a gravity compensation submodule for executing a gravity compensation mode, and a position control submodule for executing a position control mode. The beneficial technical effect of this preferred embodiment is at least that there are multiple control submodules corresponding to multiple control modes to choose from so as to more flexibly cope with various situations.
[0036] According to a preferred embodiment of the present invention, the motion control mode switching module switches the current motion control mode of the end effector to the motion control mode determined in the task planning module by switching the submodules. The beneficial technical effect of this preferred embodiment is at least that the motion control mode switching can autonomously switch to a suitable motion control mode according to the current situation.
[0037] According to a preferred embodiment of the present invention, the task-level instruction includes one or more of the following items: moving the target object, processing the target object, interacting with the target object and avoiding hitting the target object, and avoiding hitting non-target objects. The beneficial technical effect of this preferred embodiment is at least that the control device can receive and implement multiple task-level instructions.
[0038] According to a preferred embodiment of the present invention, when the task-level instruction includes moving the target object, the environment monitoring module includes identifying the target object, calculating the current position and the target position of the target object. The beneficial technical effect of this preferred embodiment is at least that the control device can more accurately control the end effector according to the output of the environment monitoring module.
[0039] According to a preferred embodiment of the present invention, when the task-level instruction includes processing a target object, the environment monitoring module is used to identify the target object, determine the part of the target object to be processed, calculate the position and orientation of the part to be processed, and calculate the current position and orientation of the end effector. The beneficial technical effect of this preferred embodiment is at least that the control device can process the part of the target object to be processed more accurately according to the output of the environment monitoring module.
[0040] According to a preferred embodiment of the present invention, when the task-level instruction includes interacting with a target object and avoiding hitting the target object, the environment monitoring module is used to identify the target object, calculate the position of the target object, calculate the current position of the end effector, and calculate the interaction position between the end effector and the target object. The beneficial technical effect of this preferred embodiment is at least that the control device can make the end effector avoid hitting the target object while moving quickly to the target position according to the output of the environment monitoring module.
[0041] According to a preferred embodiment of the present invention, when the task-level instruction includes avoiding collision with non-target objects, the environment monitoring module is used to monitor in real time whether there are non-target objects within a predetermined range around the multi-axis motion machine (or the end effector). The beneficial technical effect of this preferred embodiment is at least that the control device can enter the required control mode in advance according to the output of the environment monitoring module.
[0042] In another aspect, the present invention also provides a multi-axis motion control device for controlling a multi-axis motion machine, the multi-axis motion control device comprising a memory and a controller, wherein the memory is used to store a computer program, and the controller is configured to implement the control method as described in the above embodiment when executing the computer program. The multi-axis motion control device can autonomously switch between multiple motion control modes according to task-level instructions.
[0043] Furthermore, the present invention also provides a computer-readable storage medium, wherein a computer program is stored in the computer-readable storage medium, and when the computer program is executed by a controller, the control method described in the above embodiment can be implemented. The computer-readable storage medium can implement a control method for autonomously switching between multiple motion control modes according to task-level instructions. BRIEF DESCRIPTION OF THE DRAWINGS
[0044] The following drawings are intended only to schematically illustrate and explain the present invention and do not limit the scope of the present invention. To simplify the drawings, only the parts related to the present invention are schematically indicated in each figure, and they do not represent the actual structure or architecture of the product. To make the drawings easier to understand, parts with the same structure or function in some figures are marked with the same or similar reference numerals. In addition, to simplify the drawings, only one of the parts with the same structure or function in some figures is schematically indicated or marked. In the drawings,
[0045] Figure 1 The flowchart of a multi-axis motion control method for controlling a multi-axis motion machine according to a preferred embodiment of the present invention is schematically shown.
[0046] Figure 2 The first application scenario of the control method of the present invention is schematically illustrated.
[0047] Figure 3 The control method of the present invention is schematically shown to be applied to Figure 2 Flowchart for the scenario shown.
[0048] Figure 4 A second application scenario of the control method of the present invention is schematically illustrated.
[0049] Figure 5 The control method of the present invention is schematically shown to be applied to Figure 4 Flowchart for the scenario shown.
[0050] Figure 6 The third application scenario of the control method of the present invention is schematically shown.
[0051] Figure 7 The control method of the present invention is schematically shown to be applied to Figure 6 Flowchart for the scenario shown.
[0052] Figure 8 The figure schematically shows an architecture diagram of a multi-axis controller for controlling a multi-axis motion machine according to a preferred embodiment of the present invention.
[0053] Reference numerals: 10 - end effector; 20 - sensor; 30 - multi-axis motion control device; 40, 50 - pallet; 41, 42, 51, 52 - workpiece; 60 - intruder; 70 - operator; 80 - part to be processed; 81 - part to be processed DETAILED DESCRIPTION
[0054] In order to have a clearer understanding of the technical features, purposes and effects of the present invention, the specific embodiments of the present invention are now described with reference to the accompanying drawings. In the drawings, the same reference numerals represent components with the same structure or similar structures but the same functions.
[0055] In this document, “exemplary” means “serving as an example, instance or illustration”, and any diagram or implementation described in this document as “exemplary” should not be interpreted as a more preferred or more advantageous technical solution.
[0056] The method of the present invention is mainly aimed at multi-axis motion machines or multi-axis motion robots having an end effector (i.e., having multiple joints) that can move around multiple axes, such as multi-axis manipulators and collaborative robots, etc., and is used to autonomously switch the motion control mode of the manipulator for different task types.
[0057] Figure 1The general flow chart of the multi-axis motion control method according to the present invention is shown, which can be applied to the multi-axis motion machine or multi-axis motion robot as described above to determine and switch the motion control mode of its end effector. As shown in the figure, the control method of the present invention includes a main task S1 and three subtasks S2, S3, and S4. In the main task S1, firstly, in step 110, a task-level instruction is received, for example, moving or processing a target object (workpiece) through an operator, avoiding collision with a non-target object (intruder), interacting with a target object (workpiece, operator) and avoiding collision with the target object to cause injury, etc. The "target object" in this article generally refers to a planned object related to the task, such as a workpiece, a pallet, an operator, etc., and a "non-target object" generally refers to a non-planned object unrelated to the task, such as an intruder, etc. Specifically, the task-level instruction can be to move the workpiece from one pallet (current position) to another pallet (target position), process the workpiece (e.g., drill a hole or screw a screw on a surface of the workpiece), avoid hitting an intruder who enters a certain range around the multi-axis motion machine or the end effector, deliver the workpiece to the operator or take the workpiece from the operator and avoid hitting the operator, and any combination of the above instruction types. In step 120, the received instruction is interpreted and decomposed into three subtasks: the environment monitoring subtask S2, the motion control subtask S3, and the motion control mode switching subtask S4. In the subsequent operation steps, the control in the motion control subtask S3 and the motion control mode switching subtask S4 is corrected in real time by the dynamic environment information obtained in the environment monitoring subtask S2. In step 130, the motion trajectory of the end effector is planned according to the static environment information obtained in the environment monitoring subtask (e.g., the current position and posture of the workpiece to be moved and the target position and posture, the position of the operator, etc.).
[0058] In the environmental monitoring subtask S2, in step 210, environmental data around the multi-axis motion machine is acquired through one or more sensors, and the environmental data may be, for example, color or 3D images. In step 220, the target object (e.g., workpiece or operator) is identified and / or whether non-target objects (e.g., intruders) exist within the safety range is monitored, which may be achieved by using machine learning methods (e.g., deep learning, pattern-based object recognition), etc. In step 230, the position of the above-mentioned object is calculated, and the position of the target object is provided as static environmental information to step 130 to plan the motion trajectory of the end effector. At the same time, whether there is a non-target object and the position of the non-target object (if any), whether the end effector has approached or left the operator, whether it has reached the processing start position and the processing end position, etc. are provided as dynamic environmental information to steps 320 and 340 for determining whether the switching conditions are met.
[0059] In the motion control subtask S3, in step 310, the initial motion control mode of the end effector is determined. The initial motion control mode is generally a position control mode, so that the end effector moves quickly to a predetermined position in response to the instruction. In step 320, it is determined whether the switching condition is met based on the dynamic environment data obtained in step 230. If the switching condition is not met, it returns to step 310 and continues to execute the position control mode. If the switching condition is met, it enters the motion control mode switching subtask S4. In step 410, a suitable motion control mode is selected based on the instruction type. For example, for instructions to move a target object (such as a workpiece), a position control mode can be selected; for instructions to process (such as drilling or screwing) a target object (such as a workpiece), a Cartesian space impedance control mode can be selected; for instructions to avoid hitting a non-target object (such as an intruder), a gravity compensation control mode can be selected; for instructions to interact with a target object (such as a workpiece, an operator, etc.) and avoid hitting the target object, an axis space impedance control mode can be selected. In step 420, the control parameters can be adjusted according to the formulas corresponding to different motion control modes, and in step 430, the motion control mode with appropriate control parameters is provided to step 330 in subtask S3. At this point, the motion control mode of the operator has been switched to the required control mode. In step 340 of the motion control subtask S3, it is further determined whether the above switching conditions are still met. If they are still met, the motion control mode that has just been switched is still adopted; if they are no longer met, step 350 is returned to restore the initial motion control mode. When the above-mentioned types of instructions exist in a combined form, the priorities of the above-mentioned multiple motion control modes can be set to determine the control mode that is switched first. For example, when the instruction is to deliver the workpiece from the pallet to the operator and avoid hitting the intruder that may appear during this period, if the end effector has approached the interactive position with the operator and the intruder appears in the safety range at the same time, the motion control mode is preferentially switched from the position control mode to the gravity compensation control mode (avoid hitting the intruder), and when the intruder leaves the safety range, the gravity compensation control mode is switched to the axis space impedance control mode (interacting with the operator and avoiding hitting the operator).
[0060] The control method of the present invention will be described below by way of examples in combination with several different application scenarios. Figure 2 and Figure 3 The first application scenario and the corresponding control method flow chart are schematically shown. Figure 2 As shown, the motion control of the end effector 10 is achieved by the multi-axis motion control device 30 according to the present invention. Figure 2In the application scenario shown, the task instruction is to move the workpieces 41 and 42 from the pallet 40 to the pallet 50 by the end effector 10, and place them at a position that does not interfere with the workpieces 51 and 52 already in the pallet 50, and to stop the end effector 10 and avoid hitting the intruder when an intruder enters the safety range around the multi-axis motion machine or its end effector 10 or comes into contact with the end effector 10. Figure 3 To further illustrate, in the main task S11, the instruction is received in step 111, and then the instruction is interpreted and decomposed into three subtasks in step 121: an environment monitoring subtask S21, a motion control subtask S31, and a motion control mode switching subtask S41. In step 131, the motion trajectory of the end effector 10 is planned based on the static environment information obtained in the environment monitoring subtask, that is, the positions of the pallet 40, the workpieces 41 and 42, the pallet 50, the workpieces 51 and 52, etc.
[0061] In the environmental monitoring subtask S21, in step 211, environmental data around the multi-axis motion machine, such as a three-dimensional image of a pallet, a workpiece, and an intruder, is acquired through one or more sensors 20. In step 221, the type of the workpiece to be moved and the type of the existing workpiece in the target pallet are identified to determine the size, shape, etc. of the workpiece. In step 231, the pick-up and delivery position of the workpiece is calculated and the result is fed back to step 131 to plan the motion trajectory of the end effector. In step 241, it is monitored whether there is an intruder 60 within the safety range of the end effector 10, and the information is provided to the motion control subtask S31.
[0062] In the motion control subtask S31, the initial motion control mode of the end effector is determined in step 311. The initial motion control mode is generally a position control mode, so that the end effector moves quickly to a predetermined position in response to the instruction. In step 321, it is determined whether there is an intruder based on the monitoring result of step 241. If not, the end effector 10 continues to be controlled in the position control mode; if there is an intruder, the end effector 10 stops moving in step 331 and enters the motion control mode switching subtask S41. In step 411, the gravity compensation control mode is selected. In step 421, the corresponding control parameters are adjusted according to the following formula (1), and in step 431, the gravity compensation control mode with appropriate control parameters is provided to step 341 in subtask S31. At this point, the motion control mode of the end effector has been switched to the gravity compensation control mode. In the gravity compensation control mode, if the intruder comes into contact with the end effector, the end effector will not apply (feedback) any force to the intruder or only apply (feedback) a very small force. In step 351 of the motion control subtask S31, it is further determined whether the above switching condition is still satisfied. If it is still satisfied, the gravity compensation control mode is still adopted; if it is no longer satisfied, it goes to step 361 to restore the initial motion control mode, that is, the position control mode, and the above steps will be repeated.
[0063]
[0064] In formula (1) corresponding to the gravity compensation control mode, τ is the output torque of the power system; g(q) is the gravity torque vector function; is the joint friction function; q is the joint position; is the joint velocity.
[0065] After the above control mode switching, the output of the motion control device 30 will change from the motor speed setting value plus torque feedforward to the motor torque, so that no additional force will be applied when colliding with the intruder, thereby avoiding harm to the intruder.
[0066] Figure 4 and Figure 5 The second application scenario and the corresponding control method flow chart are schematically shown. Figure 4 As shown, the motion control of the end effector 10 is achieved by the multi-axis motion control device 30 according to the present invention. Figure 4In the application scenario shown, the task instruction is to deliver the workpieces 41, 42 from the pallet 40 to the operator 70 through the end effector 10, and the operator 70 places the workpieces 41, 42 into the pallet 50. The difference from the first scenario described above is that the end effector 10 needs to actively deliver the workpiece to the operator 70 and cannot hurt the operator during the delivery process, and the operator 10 must maintain efficient operation (do not stop moving). Figure 5 To further illustrate, in the main task S12, the instruction is received in step 112, and then the instruction is interpreted and decomposed into three subtasks in step 122: an environment monitoring subtask S22, a motion control subtask S32, and a motion control mode switching subtask S42. In step 132, the motion trajectory of the end effector 10 is planned based on the static environment information obtained in the environment monitoring subtask, i.e., the positions of the pallet 40, the workpieces 41 and 42, the pallet 50, the workpieces 51 and 52, and the operator 70.
[0067] In the environmental monitoring subtask S22, in step 212, environmental data around the multi-axis motion machine, such as a three-dimensional image of a pallet, a workpiece, and an operator, is acquired through one or more sensors 20. In step 222, obstacles and the type of workpiece to be moved are identified to determine the size and shape of the workpiece. In step 232, the operator 70 is identified, and the operator can be identified from beginning to end using image processing methods or machine learning methods, and the information is provided to the motion control subtask S32. In step 242, the pick-up and delivery position of the workpiece and the interaction position with the operator 70 are calculated and the results are fed back to step 132 to plan the motion trajectory of the end effector.
[0068] In the motion control subtask S32, the initial motion control mode of the end effector is determined in step 312. The initial motion control mode is generally a position control mode, so that the end effector moves quickly to a predetermined position in response to the instruction. In step 322, it is determined whether the operator 70 has been approached based on the recognition result of step 232. If it is not approached, the end effector 10 continues to be controlled in the position control mode; if it is approached, the motion control mode switching subtask S42 is entered. In step 412, the axis system spatial impedance control mode is selected, that is, the end effector 10 has a certain elasticity like a spring when moving around the axis (joint), and can feedback a certain amount of force to the operator when it comes into contact with the operator 70 without causing injury to the operator 70. In step 422, the corresponding control parameters are adjusted according to the following formula (2), and in step 432, the axis system spatial impedance control mode with appropriate control parameters is provided to step 332 in subtask S32. At this point, the motion control mode of the operator has been switched to the axis system spatial impedance control mode. In step 342 of the motion control subtask S32, it is further determined whether the end effector 10 has moved away from the operator 70. If it has not moved away, the axis space impedance control mode is still adopted; if it has moved away, it goes to step 352 to restore the initial motion control mode, that is, the position control mode, and the above steps will be repeated.
[0069]
[0070] In formula (2) corresponding to the shaft system spatial impedance control mode, τ is the output torque of the power system; M(q) is the inertia matrix; is the Coriolis / centrifugal matrix; g(q) is the gravitational torque vector function; is the joint friction function; k p and k d are the joint stiffness matrix and joint damping matrix, respectively, which can be configured manually; q is the actual joint position; is the actual joint velocity; q * is the ideal joint position; is the ideal joint speed; is the ideal joint acceleration.
[0071] After the above control mode switching, the output of the motion control device 30 will change from the motor speed setting value plus torque feedforward to the motor torque, so that when it comes into contact with the operator, it will yield in the direction of movement around the axis (joint) to avoid harming the operator.
[0072] Figure 6 and Figure 7 The third application scenario and the corresponding control method flow chart are schematically shown. Figure 6As shown, the motion control of the end effector 10 is achieved by the multi-axis motion control device 30 according to the present invention. Figure 6 In the application scenario shown, the task instruction is to use the end effector (drill or screwdriver) 10 to drill a hole or screw a screw on the inclined surface 81 of the workpiece 80 to be processed in a direction perpendicular to the inclined surface 81 with a predetermined force. Figure 6 As shown, the coordinate system represented by x0 and z0 is the coordinate system of the base of the collaborative robot with the end effector 10, and the coordinate system represented by x and z is the coordinate system of the end effector.
[0073] Combination Figure 7 To further illustrate, in the main task S13, the instruction is received in step 113, and then the instruction is interpreted and decomposed into three subtasks in step 123: an environment monitoring subtask S23, a motion control subtask S33, and a motion control mode switching subtask S43. In step 133, the motion trajectory of the drill or screwdriver 10 is planned based on the static environment information obtained in the environment monitoring subtask, i.e., the position of the part 80 to be processed, the position and orientation of the part 81 to be processed, etc.
[0074] In the environmental monitoring subtask S23, in step 213, environmental data around the multi-axis motion machine is acquired through one or more sensors 20. In step 223, image processing and machine learning methods can be used to identify the surface to be processed and the inclination angle of the surface. In step 233, the entry point is calculated, that is, the position at a certain distance vertically above the surface to be processed; and the target point (that is, the ideal hole depth) is calculated or the feedback torque of a predetermined size is determined, and the calculation result is fed back to step 133 to plan the motion trajectory of the end effector. In step 243, the end effector (drill or screwdriver) coordinate system is converted into a base coordinate system and the result is provided to the motion control subtask S33.
[0075] In the motion control subtask S33, the initial motion control mode of the end effector 10 is determined in step 313. The initial motion control mode is generally a position control mode, so that the end effector moves quickly to a predetermined position, i.e., the entry point, in response to the instruction. In step 323, it is determined whether the end effector has reached the entry point (i.e., the position where drilling or screwing begins) based on the calculation result of step 243. If not, the end effector 10 continues to be controlled in the position control mode; if it has reached, the motion control mode switching subtask S43 is entered. In step 413, the Cartesian space impedance control mode is selected so that the end effector 10 has a certain elasticity in its forward direction like a spring. In step 423, the corresponding control parameters are adjusted according to the following formula (3), and in step 433, the Cartesian space impedance control mode with appropriate control parameters is provided to step 333 in subtask S33. At this point, the motion control mode of the end effector 10 has been switched to the Cartesian space impedance control mode. In step 343 of the motion control subtask S33, it is further determined whether the end effector 10 has reached the target point, for example, whether the drill has reached the predetermined drilling depth or whether the screw being screwed has fed back a predetermined torque to the end effector. If not, the Cartesian space impedance control mode is still adopted to continue the operation; if it has reached, it goes to step 353 to restore the initial motion control mode, that is, the position control mode withdraws the end effector 10. The above steps can be repeated to drill multiple holes or screw multiple screws.
[0076]
[0077] In formula (3) corresponding to the Cartesian space impedance control mode, τ is the output torque of the power system; g(q) is the gravity torque vector function; is the joint friction function; J(q) is the Jacobian matrix; Λ(x) is the inertia matrix; is the nonlinear term in Cartesian space; D d is the Cartesian space damping; K d is the Cartesian space stiffness; F c is the force applied by the end effector; K f To determine F c The force selection matrix in the direction of ; q is the joint position; is the joint velocity; x is the actual position of the end effector in Cartesian space; x d is the planned position of the end effector in Cartesian space; is the planned velocity of the end effector in Cartesian space; is the planned acceleration of the end effector in Cartesian space; is the position error of the end effector in Cartesian space.
[0078] On the other hand, the present invention also provides a multi-axis motion control device or controller for controlling a multi-axis motion machine based on the above control method, and its schematic architecture is as follows: Figure 8 As shown in the dotted box. The device includes an environment monitoring module, a task planning module, a motion control module and a motion control mode switching module. Among them, the environment monitoring module 2000 is used to collect environmental data and identify targets (for example, workpieces, operators, intruders and obstacles, etc.), and the environmental data collected by it is one of the basic information used by the task planning module to plan tasks. The environment monitoring module 2000 sends a sensor control instruction 2010 to the sensor 20, and the sensor 20 feeds back the sensing data 21 to the environment monitoring module 2000 according to the instruction 2010. The task planning module is used to decompose the received task-level instructions into multiple subtasks and perform autonomous task planning (for example, planning the motion trajectory of the end operator, etc.) according to the static environment information collected by the environment monitoring module 2000. The task planning module includes an autonomous task planning module 1100 for interpreting instructions and splitting subtasks and an autonomous path planning module 1200 for planning the motion trajectory of the end operator. The motion control module is used to determine the motion control mode for controlling the end effector in real time according to the dynamic environment information obtained in the environment monitoring module and to execute the motion control mode for controlling the end effector, which includes a forward & inverse kinematics module 3100, a kinematics interpolation module 3200, a dynamics model module 3300 and a position feedback module 3400, wherein the dynamics model module 3300 includes an impedance control submodule 3310 for performing impedance control, a gravity compensation submodule 3320 for performing gravity compensation control and a torque feedforward submodule or a position control submodule 3330 for performing position control. The motion control mode switching module is used to switch the current motion control mode of the end effector to the determined motion control mode and adjust the corresponding control parameters. The motion control mode switching module includes a motion control mode selection module 4100 for selecting a motion control mode and a control parameter adjustment module 4200 for adjusting control parameters. The motion control mode switching module also includes a motion control mode switching switch 4300 for switching the motion control mode and a communication module 4400 for communicating with the servo drive device 90. The servo drive device 90 is connected to the motor 100 for controlling the motor 100. The servo drive device 90 is also used to feed back the actual position, speed and torque data 91 of the motor to the communication module 4400.
[0079] like Figure 8As shown, the forward & inverse kinematics module 3100 and the kinematics interpolation module 3200 are used to interpolate and calculate the motion trajectory of the end effector and regularly (for example, every 2ms) generate the joint theoretical position, velocity and acceleration 3210 to provide to the dynamic model module 3300. The impedance control submodule 3310 and the gravity compensation submodule 3320 calculate the joint theoretical torque according to the joint theoretical position, velocity and acceleration 3210 and output the motor torque 3311 and 3321 for controlling the motor 100 respectively. The position control submodule 3330 calculates the joint theoretical torque according to the joint theoretical position, velocity and acceleration 3210 and outputs the motor feedforward torque 3331 for controlling the motor 100 to improve the dynamic performance of the system. The position feedback module 3400 is used to monitor the position error between the theoretical position and the actual position of the end effector, calculate the theoretical speed adjustment value 3410 and add it to the motor speed setting value 3420.
[0080] On the other hand, the present invention also provides a multi-axis motion control device for controlling a multi-axis motion machine, the multi-axis motion control device comprising a memory and a controller, wherein the memory is used to store a computer program, and the controller is configured to be able to implement the control method described above when executing the computer program.
[0081] Furthermore, the present invention also provides a computer-readable storage medium, in which a computer program is stored. When the computer program is executed by a controller, the control method as described above can be implemented.
[0082] It should be understood that although this specification is described according to various embodiments, not every embodiment contains only one independent technical solution. This narrative method of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment may also be appropriately combined to form other implementation methods that can be understood by those skilled in the art.
[0083] The above description is only an illustrative embodiment of the present invention and is not intended to limit the scope of the present invention. Any equivalent changes, modifications and combinations made by any person skilled in the art without departing from the concept and principle of the present invention shall fall within the scope of protection of the present invention.
Claims
1. A multi-axis motion control method for controlling a multi-axis motion machine, the multi-axis motion machine comprising an end effector (10) capable of moving around a plurality of axes, the method include: receiving task-level instructions; Decomposing the task-level instruction into a plurality of subtasks, the plurality of subtasks including an environment monitoring subtask, in which environmental information around the multi-axis motion machine is sensed and determined; when the task-level instruction includes processing a target object, the environment monitoring subtask includes identifying the target object, determining a portion of the target object to be processed, calculating a position and orientation of the portion to be processed, and calculating a current position and orientation of the end effector (10); Planning a motion trajectory, planning the motion trajectory of the end effector (10) according to the static environment information obtained in the environment monitoring subtask; The multiple subtasks also include: A motion control subtask, which determines in real time a motion control mode for controlling the end effector (10) according to the dynamic environment information obtained in the environment monitoring subtask; A motion control mode switching subtask, switching the current motion control mode to the motion control mode determined in the motion control subtask and adjusting corresponding control parameters; The motion control subtask includes switching the motion control mode for controlling the end effector (10) to a Cartesian space impedance control mode according to the current position of the end effector (10); the motion control mode switching subtask includes adjusting control parameters corresponding to the Cartesian space impedance control mode; The motion control mode switching subtask includes adjusting the control parameters corresponding to the Cartesian space impedance control mode according to the following formula: Where: τ is the output torque of the power system; g(q) is the gravity torque vector function; is the joint friction function; J(q) is the Jacobian matrix; Λ(x) is the inertia matrix; is a nonlinear term in Cartesian space; D d is the Cartesian space damping; K d is the Cartesian space stiffness; F c a force applied by the end effector; K f To determine F c The force selection matrix in the direction of ; q is the joint position; is the joint velocity; x is the actual position of the end effector in Cartesian space; x d is the planned position of the end effector in Cartesian space; is the planned velocity of the end effector in Cartesian space; is the planned acceleration of the end effector in Cartesian space; is the position error of the end effector in Cartesian space.
2. The multi-axis motion control method according to claim 1, in, The initial motion control mode for controlling the end effector (10) is a position control mode.
3. The multi-axis motion control method according to claim 2, in, The task-level instructions include one or more of the following items: moving the target object, processing the target object, interacting with the target object and avoiding hitting the target object, and avoiding hitting non-target objects.
4. The multi-axis motion control method according to claim 3, in, When the task-level instruction includes moving a target object, the environment monitoring subtask includes identifying the target object, and calculating the current position and the target position of the target object.
5. The multi-axis motion control method according to claim 4, in, The motion control subtask includes determining that the motion control mode for controlling the end effector (10) is a position control mode.
6. The multi-axis motion control method according to claim 3, in, When the task-level instruction includes interacting with a target object and avoiding hitting the target object, the environmental monitoring subtask includes identifying the target object, calculating the position of the target object, calculating the current position of the end effector (10), and calculating the interaction position between the end effector (10) and the target object.
7. The multi-axis motion control method according to claim 6, in, The motion control subtask includes switching the motion control mode for controlling the end effector (10) to the shaft system spatial impedance control mode according to the current position of the end effector (10), and the motion control mode switching subtask includes adjusting the control parameters corresponding to the shaft system spatial impedance control mode according to the following formula: Where: τ is the output torque of the power system; M(q) is the inertia matrix; for the Coriolis / centrifugal matrix; g(q) is the gravity torque vector function; is the joint friction function; k p is the joint stiffness matrix; k d is the joint damping matrix; q is the actual joint position; is the actual joint velocity; q * is the ideal joint position; is the ideal joint speed; is the ideal joint acceleration.
8. The multi-axis motion control method according to claim 3, in, When the task-level instruction includes avoiding hitting non-target objects, the environment monitoring subtask includes real-time monitoring whether there are non-target objects within a predetermined range around the multi-axis motion machine.
9. The multi-axis motion control method according to claim 8, in, The motion control subtask includes switching the motion control mode for controlling the end effector (10) to a gravity compensation mode, and the motion control mode switching subtask includes adjusting the control parameters corresponding to the gravity compensation mode according to the following formula: Where: τ is the output torque of the power system; g(q) is the gravity torque vector function; is the joint friction function; q is the joint position; is the joint velocity.
10. A multi-axis motion control device for controlling a multi-axis motion machine, the multi-axis motion machine comprising an end effector (10) capable of moving around a plurality of axes, the multi-axis motion control device (30) comprising the following modules: An environmental monitoring module, used to sense and determine environmental information around the multi-axis motion machine; a task planning module, for decomposing the received task-level instruction into a plurality of subtasks and planning the motion trajectory of the end effector (10) according to the static environment information obtained by the environment monitoring module; when the task-level instruction includes processing a target object, the environment monitoring subtask includes identifying the target object, determining a part of the target object to be processed, calculating the position and orientation of the part to be processed, and calculating the current position and orientation of the end effector (10); A motion control module, used for determining in real time a motion control mode for controlling the end effector (10) according to the dynamic environment information obtained in the environment monitoring module, and for executing the motion control mode for controlling the end effector (10); A motion control mode switching module, used to switch the current motion control mode of the end effector (10) to the determined motion control mode and adjust the corresponding control parameters; The motion control subtask includes switching the motion control mode for controlling the end effector (10) to a Cartesian space impedance control mode according to the current position of the end effector (10); the motion control mode switching subtask includes adjusting control parameters corresponding to the Cartesian space impedance control mode; The motion control mode switching subtask includes adjusting the control parameters corresponding to the Cartesian space impedance control mode according to the following formula: Where: τ is the output torque of the power system; g(q) is the gravity torque vector function; is the joint friction function; J(q) is the Jacobian matrix; Λ(x) is the inertia matrix; is a nonlinear term in Cartesian space; D d is the Cartesian space damping; K d is the Cartesian space stiffness; F c a force applied by the end effector; K f To determine F c The force selection matrix in the direction of ; q is the joint position; is the joint velocity; x is the actual position of the end effector in Cartesian space; x d is the planned position of the end effector in Cartesian space; is the planned velocity of the end effector in Cartesian space; is the planned acceleration of the end effector in Cartesian space; is the position error of the end effector in Cartesian space.
11. The multi-axis motion control device according to claim 10, in, The motion control module includes an impedance control submodule for executing an impedance control mode, a gravity compensation submodule for executing a gravity compensation mode, and a position control submodule for executing a position control mode.
12. The multi-axis motion control device according to claim 11, in, The motion control mode switching module switches the current motion control mode of the end effector (10) to the motion control mode determined in the task planning module by switching the submodules.
13. The multi-axis motion control device according to claim 10, in, The task-level instructions include one or more of the following items: moving the target object, processing the target object, interacting with the target object and avoiding hitting the target object, and avoiding hitting non-target objects.
14. The multi-axis motion control device according to claim 13, in, When the task-level instruction includes moving a target object, the environment monitoring module includes identifying the target object, and calculating the current position and the target position of the target object.
15. The multi-axis motion control device according to claim 13, in, When the task-level instruction includes interacting with a target object and avoiding hitting the target object, the environment monitoring module is used to identify the target object, calculate the position of the target object, calculate the current position of the end operator (10), and calculate the interaction position between the end operator (10) and the target object.
16. The multi-axis motion control device according to claim 13, in, When the task-level instruction includes avoiding hitting non-target objects, the environment monitoring module is used to monitor in real time whether there are non-target objects within a predetermined range around the multi-axis motion machine.
17. A multi-axis motion control device for controlling a multi-axis motion machine, the multi-axis motion control device (30) comprising a memory and a controller, in, The memory is used to store a computer program, and the controller is configured to implement the method according to any one of claims 1 to 9 when executing the computer program.
18. A computer-readable storage medium, wherein a computer program is stored in the computer-readable storage medium, and when the computer program is executed by a controller, the method according to any one of claims 1 to 9 can be implemented.
Citation Information
Patent Citations
Control devices and methods for robotic arms, assembly robots, control programs for robotic arms, and integrated circuits for robotic arm control.
CN102292194A
Robotic arm control device and control method, robot, robotic arm control program and integrated circuit for robotic arm control
CN102300679A
Backfill scheduling method and system for embarrassingly parallel jobs
CN104731642A
Control method of spatial dual-arm robot for capturing tumbling targets
CN109015658A
Multiple priority operational space impedance control
US20100161127A1
