CN112894827B - Method, system and device for controlling motion of mechanical arm and readable storage medium - Google Patents
Method, system and device for controlling motion of mechanical arm and readable storage medium Download PDFInfo
- Publication number
- CN112894827B CN112894827B CN202110210584.5A CN202110210584A CN112894827B CN 112894827 B CN112894827 B CN 112894827B CN 202110210584 A CN202110210584 A CN 202110210584A CN 112894827 B CN112894827 B CN 112894827B
- Authority
- CN
- China
- Prior art keywords
- motion
- joint
- controller
- mechanical arm
- asynchronous timer
- Prior art date
- Legal status (The legal status 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 status listed.)
- Active
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
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
Abstract
The application discloses a mechanical arm motion control method, wherein a real-time operating system is established on a motion controller, and a communication function is realized. Based on three asynchronous timers, the motion controller receives a motion control command sent by an upper computer, determines a motion path of the mechanical arm according to the motion control command, generates joint control commands of joint controllers corresponding to joints of the mechanical arm according to the motion path, sends the joint control commands to the corresponding joint controllers, and realizes quick response of the motion control command of the upper computer and real-time control of joints of the mechanical arm through multithread concurrency, so that the control precision and the control response rate are greatly improved, multiple motion modes are facilitated to be realized, and the working efficiency of the mechanical arm is improved. The application also discloses a mechanical arm motion control system, a device and a readable storage medium, which have the beneficial effects.
Description
Technical Field
The present disclosure relates to the field of mechanical arm motion control, and in particular, to a method, a system, an apparatus, and a readable storage medium for controlling mechanical arm motion.
Background
The multi-degree-of-freedom robot is a mechanical electronic device capable of simulating the functions of a human arm, a wrist and a hand, and can move any object or tool according to the time-varying requirement of a space pose so as to meet the operation requirement of certain industrial production. The degree of freedom of the manipulator refers to the flexibility of movement of the manipulator of the conveying mechanism. Each degree of freedom of the manipulator is achieved by an independent drive joint of its manipulator.
The nine-degree-of-freedom serial mechanical arm is a super-redundant mechanical arm, and has higher flexibility and redundancy compared with the traditional six-degree-of-freedom serial mechanical arm. The flexibility means that the space for solving the series arm is not unique, the most suitable pose can be selected according to the working requirement, and the functions of flexible operation, obstacle avoidance and the like in a narrow space can be completed. Redundancy means that when some joints fail, the rest joints can still ensure the normal work of the mechanical arm. The nine-degree-of-freedom series mechanical arm has great application potential in the fields of space application, deep sea detection, multi-arm cooperative work and the like.
However, most of the existing robot control systems are six-degree-of-freedom series arm control systems, and cannot control a nine-degree-of-freedom series mechanical arm. A few control systems designed for specific robots only meet basic positioning requirements, such as executing a specific motion control script, failing to implement real-time motion control, and failing to implement complex motions in combination with sensors.
Therefore, it is a technical problem to be solved by those skilled in the art to provide a robot arm motion control scheme capable of implementing real-time motion control in cooperation with a super-redundant robot arm.
Disclosure of Invention
The application aims to provide a mechanical arm motion control method, a system and a device and a readable storage medium, which are used for realizing real-time motion control by matching with a super-redundant mechanical arm.
In order to solve the above technical problem, the present application provides a method for controlling motion of a robot arm, based on a motion controller, including:
receiving a motion control instruction sent by an upper computer;
determining a motion path of the mechanical arm according to the motion control instruction, and generating joint control instructions of joint controllers corresponding to joints of the mechanical arm according to the motion path;
the joint control instruction is issued to the corresponding joint controller;
the motion controller runs a first asynchronous timer used for realizing communication between the motion controller and the upper computer, a second asynchronous timer used for realizing communication between the motion controller and each joint controller, and a third asynchronous timer used for executing a motion control program of the mechanical arm; the first asynchronous timer, the second asynchronous timer, and the third asynchronous timer have equal priority.
Optionally, the method further includes:
monitoring an emergency stop signal;
and when the emergency stop signal is received, sending an emergency stop control command to the joint controller.
Optionally, the motion controller further operates a fourth asynchronous timer for implementing communication between the motion controller and a force sensor disposed at the end of the mechanical arm;
correspondingly, the generating joint control commands of the joint controllers corresponding to the joints of the mechanical arm according to the motion path specifically includes:
compensating the motion path according to the stress information of the tail end of the mechanical arm fed back by the force sensor to obtain a compensated motion path;
generating joint control instructions for the joint controllers according to the compensated motion path;
wherein the fourth asynchronous timer has equal priority to the first asynchronous timer.
Optionally, a communication protocol between the motion controller and the upper computer is a transmission control protocol; the communication protocol between the motion controller and each joint controller is a control local area network communication protocol; the communication protocol between the motion controller and the force sensor is a user datagram protocol.
Optionally, the determining, according to the motion control instruction, a motion path of the robot arm, and generating, according to the motion path, a joint control instruction for a joint controller corresponding to each joint of the robot arm specifically include:
calculating to obtain Cartesian space coordinates of the tail end of the mechanical arm according to the joint angle of each joint fed back by each joint controller;
determining the tail end motion path of the tail end of the mechanical arm in a Cartesian space according to the motion control instruction;
obtaining joint movement paths of all joints according to inverse solution of the tail end movement path;
generating a joint control command to a joint controller of the joint according to a joint movement path of the joint.
Optionally, the type of the motion control instruction specifically includes: the control system comprises a joint inching control instruction, a joint script motion control instruction, a Cartesian space inching control instruction, a Cartesian space script motion control instruction and a fixed point motion control instruction.
In order to solve the above technical problem, the present application further provides a robot arm motion control system, including: the system comprises an upper computer, a motion controller connected with the upper computer, and joint controllers connected with the motion controller and corresponding to joints of the mechanical arm;
the upper computer is used for receiving a user instruction, generating a motion control instruction according to the user instruction and sending the motion control instruction to the motion controller;
the motion controller runs a first asynchronous timer used for realizing communication between the motion controller and the upper computer, a second asynchronous timer used for realizing communication between the motion controller and each joint controller and a third asynchronous timer used for executing a motion control program of the mechanical arm, and is used for receiving a motion control command sent by the upper computer, determining a motion path of the mechanical arm according to the motion control command, generating a joint control command for each joint controller according to the motion path and sending the joint control command to the corresponding joint controller;
wherein the first asynchronous timer, the second asynchronous timer, and the third asynchronous timer have equal priority.
Optionally, the motion controller further operates a fourth asynchronous timer for implementing communication between the motion controller and a force sensor disposed at the end of the mechanical arm;
correspondingly, the motion controller generates joint control commands for the joint controllers according to the motion path, and specifically includes:
the motion controller compensates the motion path according to the stress information of the tail end of the mechanical arm fed back by the force sensor to obtain a compensated motion path;
the motion controller generates joint control instructions for the joint controllers according to the compensated motion path;
wherein the fourth asynchronous timer has equal priority to the first asynchronous timer.
In order to solve the above technical problem, the present application further provides a robot arm motion control apparatus, including:
the receiving unit is used for receiving a motion control instruction sent by the upper computer based on the first asynchronous timer;
the computing unit is used for determining a motion path of the mechanical arm according to the motion control instruction based on a second asynchronous timer and generating joint control instructions of joint controllers corresponding to joints of the mechanical arm according to the motion path;
the sending unit is used for sending the joint control command to the corresponding joint controller based on a third asynchronous timer;
the first asynchronous timer is used for realizing communication between the motion controller and the upper computer, the second asynchronous timer is used for realizing communication between the motion controller and each joint controller, and the third asynchronous timer is used for executing a motion control program of the mechanical arm; the first asynchronous timer, the second asynchronous timer, and the third asynchronous timer have equal priority.
In order to solve the above technical problem, the present application further provides a readable storage medium, on which a computer program is stored, wherein the computer program, when executed by a processor, implements the steps of the robot arm motion control method according to any one of the above items.
According to the mechanical arm motion control method, a first asynchronous timer used for achieving communication between the motion controller and an upper computer, a second asynchronous timer used for achieving communication between the motion controller and each joint controller and a third asynchronous timer used for executing a motion control program of the mechanical arm run on the motion controller, and the first asynchronous timer, the second asynchronous timer and the third asynchronous timer have equal priority, so that a real-time operating system is established on the motion controller and a communication function is achieved. Based on the three asynchronous timers, the motion controller receives a motion control instruction sent by the upper computer, determines a motion path of the mechanical arm according to the motion control instruction, generates joint control instructions of joint controllers corresponding to joints of the mechanical arm according to the motion path, sends the joint control instructions to the corresponding joint controllers, and realizes quick response of the motion control instruction of the upper computer and real-time control of joints of the mechanical arm through multithread concurrency, so that the control precision and the control response rate are greatly improved, multiple motion modes are facilitated to be realized, and the working efficiency of the mechanical arm is improved.
The application also provides a mechanical arm motion control system, a device and a readable storage medium, which have the beneficial effects and are not repeated herein.
Drawings
For a clearer explanation of the embodiments or technical solutions of the prior art of the present application, the drawings needed for the description of the embodiments or prior art will be briefly described below, it is obvious that the drawings in the following description are only some embodiments of the present application, and for those skilled in the art, other drawings can be obtained according to these drawings without creative efforts.
Fig. 1 is a flowchart of a method for controlling a motion of a robot according to an embodiment of the present disclosure;
FIG. 2 is a flow chart of another method for controlling the movement of a robotic arm according to an embodiment of the present disclosure;
fig. 3 is a schematic structural diagram of a robot motion control system according to an embodiment of the present disclosure;
fig. 4 is a schematic structural diagram of a robot arm motion control apparatus according to an embodiment of the present disclosure.
Detailed Description
The core of the application is to provide a mechanical arm motion control method, a system, a device and a readable storage medium, which are used for realizing real-time motion control by matching with a super-redundant mechanical arm.
The technical solutions in the embodiments of the present application will be described clearly and completely with reference to the drawings in the embodiments of the present application, and it is obvious that the described embodiments are only some embodiments of the present application, and not all embodiments. All other embodiments obtained by a person of ordinary skill in the art based on the embodiments in the present application without making any creative effort belong to the protection scope of the present application.
Fig. 1 is a flowchart of a method for controlling a motion of a robot arm according to an embodiment of the present disclosure.
As shown in fig. 1, based on a motion controller, a robot arm motion control method provided by the embodiment of the present application:
s101: and receiving a motion control command sent by the upper computer.
S102: and determining a motion path of the mechanical arm according to the motion control instruction, and generating a joint control instruction of a joint controller corresponding to each joint of the mechanical arm according to the motion path.
S103: and transmitting the joint control command to the corresponding joint controller.
The motion controller runs a first asynchronous timer for realizing communication between the motion controller and the upper computer, a second asynchronous timer for realizing communication between the motion controller and each joint controller and a third asynchronous timer for executing a motion control program of the mechanical arm; the first asynchronous timer, the second asynchronous timer and the third asynchronous timer have equal priority.
In the embodiment of the application, the mechanical arm motion control system mainly comprises an upper computer, a motion controller and a joint controller.
The upper computer is used for providing a human-computer interaction interface, displaying the control state of the mechanical arm, receiving a user instruction, converting the user instruction into a motion control command and sending the motion control command to the motion controller. The motion control command may include relevant parameters of the target motion (such as target position and posture, set motion speed and acceleration, etc.), system commands (such as enable, run, register read-write, joint angle acquisition switch, etc.), and the like.
The motion controller is used for operating a real-time operating system, realizing communication with an upper computer and communication with the joint controller, and realizing quick response to instructions of the upper computer and real-time control over the joint controller through multithread concurrence. In the motion control process, the motion controller is specifically applied to solving the generalized coordinates of the target pose of the mechanical arm in each joint space by performing positive and negative solution operation on the kinematics of the mechanical arm, and generating the track between the current pose of the mechanical arm and the target pose of the mechanical arm by utilizing a track interpolation algorithm.
The joint controllers are controlled to move by the motion controllers, each joint controller having a unique ID.
In specific implementation, the embodiment of the application adopts a distributed control software architecture, runs on an NI Labview real-time module, and is a real-time control system. And a first asynchronous timer, a second asynchronous timer and a third asynchronous timer are established based on an NI Labview real-time module, and other control threads can be set to realize multithreading concurrency and achieve the purpose of real-time motion control. For safety, the robot arm motion control method provided by the embodiment of the application further includes: monitoring an emergency stop signal; and when the emergency stop signal is received, sending an emergency stop control command to the joint controller. The running safety of the robot and an operator is guaranteed by arranging a control thread for monitoring and triggering an emergency stop signal, wherein the control thread has the highest priority. The first asynchronous timer, the second asynchronous timer and the third asynchronous timer are used for executing a communication task and a mechanical arm motion control task respectively, have medium priority and meet the requirements of real-time performance and controllability.
The communication framework of the embodiment of the application mainly comprises communication between the upper computer and the motion controller and communication between the motion controller and the joint controller.
The communication protocol between the motion controller and the upper computer adopts a Transmission Control Protocol (TCP). And building a UI (user interface) and a transmission control server (TCP SERVER) in the upper computer. The method comprises the steps of providing a module for an operator to input a user instruction or select an instruction option on a UI (user interface), wherein the type of the instruction option can comprise enabling, running, stopping, scramming, reading and writing a register, sending a script instruction, a mechanical arm motion mode and the like, and each type corresponds to a plurality of selectable options so as to provide the operator to input or select. The UI interface also comprises a module for displaying state information such as joint angles of the mechanical arm in real time, so that an operator can check the state information. And the upper computer generates a corresponding motion control instruction after receiving a user instruction input by an operator, and sends the corresponding motion control instruction to the motion controller by adopting a TCP (transmission control protocol). The motion controller sends state parameters of the mechanical arm, such as joint angles, joint states and the like, to the upper computer by adopting a TCP (transmission control protocol).
The motion Controller and each joint Controller realize communication based on a Control Area Network (CAN), and adopt a control Area Network communication protocol (CANopen). The motion controller receives data such as joint angles, registers and joint states sent by the joint controller by adopting a CANopen protocol, and sends a joint control instruction generated according to the motion control instruction to the joint controller.
Based on the mechanical arm motion control system, for step S101, after the operator inputs a user instruction to the upper computer, the upper computer generates a motion control instruction according to the user instruction, and sends the motion control instruction to the motion controller by using a TCP protocol. And the motion controller receives a motion control command sent by the upper computer. The motion control instruction can be a control instruction for a joint of the mechanical arm and can also be a control instruction for the tail end of the mechanical arm.
For step S102, the motion controller performs path planning according to the motion control command, so as to obtain a motion path of the mechanical arm and a motion model on the motion path. The motion controller determines a motion path of the mechanical arm, specifically, a velocity array of the mechanical arm from an initial position to a middle position is planned according to given velocity, acceleration, jerk value and the like in a motion control command, the trajectory planning comprises acceleration and deceleration processes, and after the maximum velocity is reached, the mechanical arm runs at a constant speed according to the maximum velocity. And finally, interpolating the speed array into a position array so as to control the motion of the mechanical arm. The motion model of the mechanical arm can adopt a T-shaped curve or an S-shaped curve.
In the path planning process, for a motion control command for the tail end of the mechanical arm, the motion controller needs to perform positive and negative solution operation of mechanical arm kinematics based on a mechanical arm kinematics forward solution algorithm and a mechanical arm kinematics inverse solution algorithm which are deployed in advance. The mechanical arm kinematics positive solution operation is an algorithm process of calculating Cartesian space coordinates of the tail end of the mechanical arm by using the joint angle of the mechanical arm, and the mechanical arm kinematics reverse solution operation is an algorithm process of calculating the joint angle of the mechanical arm by using the Cartesian space coordinates of the tail end of the mechanical arm. Step S102 may specifically include:
calculating to obtain Cartesian space coordinates of the tail end of the mechanical arm according to joint angles of the joints fed back by the joint controllers;
determining the tail end motion path of the tail end of the mechanical arm in a Cartesian space according to the motion control instruction;
obtaining joint movement paths of all joints according to inverse solution of the tail end movement path;
a joint control command to a joint controller of the joint is generated based on a joint movement path of the joint.
The joint control command comprises parameters such as the adjustment angle and the adjustment speed of the joint angle.
And for the motion control instruction aiming at the mechanical arm joint, the motion controller directly controls the corresponding joint according to the motion control instruction.
Because the motion control of the mechanical arm has high real-time requirement, if the execution period can be set to be 5ms and the frequency can be set to be 200hz in the embodiment of the application, the path planning algorithm and the inverse kinematics solution operation of the mechanical arm are required to be executed in one period and the result is sent to the joint controller and the upper computer, so on the basis of the embodiment of the application, the real-time motion control of the super-redundant mechanical arm can be effectively realized by optimizing the path planning algorithm and the inverse kinematics solution operation of the mechanical arm.
In step S103, the motion controller distributes the joint control command of the joint controller corresponding to each joint of the robot arm generated from the motion path to the corresponding joint controller. In the control process, the motion controller receives data such as joint angles, registers, joint states and the like sent by the joint controller and sends joint control instructions to the joint controller. Specifically, the motion controller encapsulates joint control commands (such as joint angle information) into CAN frames, sends the CAN frames to a CAN bus, each joint has a fixed ID, each joint controller receives data frames according to the ID of the corresponding joint, and the data frames are resolved into angle values, and the motion of the joint angle is planned according to the angle values. Since there is a separate joint controller for each joint, macroscopically all joint motions are performed concurrently.
According to the mechanical arm motion control method provided by the embodiment of the application, a first asynchronous timer for realizing communication between the motion controller and an upper computer, a second asynchronous timer for realizing communication between the motion controller and each joint controller and a third asynchronous timer for executing a motion control program of the mechanical arm are operated on the motion controller, and the first asynchronous timer, the second asynchronous timer and the third asynchronous timer have equal priority, so that a real-time operating system is established on the motion controller and a communication function is realized. Based on the three asynchronous timers, the motion controller receives a motion control instruction sent by the upper computer, determines a motion path of the mechanical arm according to the motion control instruction, generates joint control instructions of joint controllers corresponding to joints of the mechanical arm according to the motion path, sends the joint control instructions to the corresponding joint controllers, and realizes quick response of the motion control instruction of the upper computer and real-time control of joints of the mechanical arm through multithread concurrency, so that the control precision and the control response rate are greatly improved, multiple motion modes are facilitated to be realized, and the working efficiency of the mechanical arm is improved.
On the basis of the embodiment, in order to realize the complex motion of the mechanical arm combined with the sensor, the communication between the motion controller and the sensor of the mechanical arm can be established, and the real-time control of the motion of the mechanical arm based on the sensor is realized by the asynchronous timer with the same priority as the first asynchronous timer. The sensor may be a vision sensor, a force sensor, or the like.
Taking a force sensor as an example, in the method for controlling the motion of the mechanical arm provided in the embodiment of the present application, the motion controller further operates a fourth asynchronous timer for implementing communication between the motion controller and the force sensor disposed at the end of the mechanical arm.
Correspondingly, the step S102 of generating a joint control command for a joint controller corresponding to each joint of the robot arm according to the motion path specifically includes:
compensating the motion path according to the stress information of the tail end of the mechanical arm fed back by the force sensor to obtain a compensated motion path;
and generating joint control commands for the joint controllers according to the compensated motion path.
Wherein the fourth asynchronous timer has the same priority as the first asynchronous timer.
In a specific implementation, the motion controller and the force sensor may communicate using a User Datagram Protocol (UDP). The motion controller sends a control command to the force sensor by adopting a UDP protocol, and the force sensor transmits stress information (comprising force information and moment information) by the UDP protocol. The motion controller determines the motion condition of the mechanical arm according to joint state information fed back by the joint controller and mechanical arm stress information fed back by the force sensor, divides the mechanical arm into a plurality of states by combining a motion control command, and plans corresponding motion forms and parameter information for the mechanical arm according to the current state.
The force sensor can be specifically installed at a clamp at the tail end of the mechanical arm, and when an operator guides the mechanical arm to move through the clamp, the influence of the guidance of the operator on the movement of the mechanical arm is measured through the force sensor, so that the movement controller can make corresponding compensation calculation.
On the basis of the above embodiment, the types of the motion control command specifically include: the control system comprises a joint inching control instruction, a joint script motion control instruction, a Cartesian space inching control instruction, a Cartesian space script motion control instruction and a fixed point motion control instruction.
The joint inching control instruction and the joint script motion control instruction are control instructions for joints. The joint inching control instruction specifically gives a speed set value, an acceleration set value and the like of a single joint, after the upper computer gives a command for starting joint inching (a inching button provided by the upper computer can be clicked/pressed by a worker), the motion controller continuously plans a motion path and controls the joint to move according to the motion path until the upper computer gives a command for stopping joint inching (such as the worker lifts the inching button) and performs deceleration motion until the joint stops. The joint script motion control command specifically gives a script of a joint angular motion path, and the motion controller reads the script and controls the joint controller of the corresponding joint to move according to the joint angular data of the script.
The cartesian space inching control instruction, the cartesian space script motion control instruction and the fixed point motion control instruction are control instructions for the tail end of the mechanical arm.
The cartesian space inching comprises six motion directions which are translation and rotation in the xyz direction respectively. The Cartesian space inching control instruction specifically gives a motion direction, a motion speed and an acceleration, after the upper computer gives a command for starting Cartesian space inching (a inching button provided by the upper computer can be clicked/pressed by a worker), the motion control module continuously plans a Cartesian motion path, and mechanical arm kinematics inverse solution operation is carried out to obtain a corresponding joint angle, so that joint angle motion is controlled until the upper computer gives a command for stopping Cartesian space inching (such as the worker lifts the inching button), and deceleration motion is carried out until the motion stops. The Cartesian space script motion control command specifically gives a script of a motion path of the tail end of the mechanical arm, and after the motion controller analyzes the script, each joint of the mechanical arm is controlled to enable the tail end of the mechanical arm to move according to a set path. The fixed-point motion control instruction specifically gives a target point of the motion of the tail end of the mechanical arm, the motion controller automatically plans a motion path from the current point to the target point, and then the motion controller reversely solves a joint angle to control each joint of the mechanical arm so that the tail end of the mechanical arm moves according to a set path.
When the motion controller executes the motion control command, if the sensor exists in the mechanical arm motion control system, the motion controller carries out compensation calculation on the current motion plan according to information fed back by the sensor. For example, for a force sensor, a motion controller collects force information and moment information fed back by the force sensor positioned at the tail end of the mechanical arm in real time, converts the force information and the moment information into a tool coordinate system, calculates speed information of the tail end of the mechanical arm, calculates displacement information through difference, and finally converts the displacement information into a joint angle to control the motion of the mechanical arm.
Fig. 2 is a flowchart of another method for controlling the motion of a robot arm according to an embodiment of the present disclosure.
On the basis of the foregoing embodiments, an embodiment of the present application provides a preferable method for controlling the motion of a robot arm, as shown in fig. 2, specifically including:
s201: after the mechanical arm motion control system is powered on, the upper computer, the motion controller and each joint controller are initialized.
Specifically, the joint controller performs joint self-inspection, CAN bus initialization and register initialization; the upper computer initializes a UI interface and builds TCP SERVER; and the motion controller starts all timers and threads, establishes a UDP CLIENT access force sensor, establishes TCP CLIENT access to the upper computer, and is connected with the CAN bus to receive data frames of all the joint controllers.
S202: after the initialization is successful, the joint controller continuously sends the joint angle and the joint state to the CAN bus. And if the motion controller receives an enabling signal sent by the upper computer, entering a ready state.
S203: the motion controller judges the received instruction content, and if a motion control instruction sent by the upper computer is received, the step S204 is executed; if the force information fed back by the force sensor is received, the process proceeds to step S212.
Specifically, after the motion controller enters a ready state, the motion controller can perform read-write operation on a register of the joint controller, but cannot control the joint motion, and the motion controller and each joint controller automatically calibrate a zero position register and set the current position as a zero position until a motion control instruction or stress information is received. The force information includes force information and moment information.
S204: the motion controller judges the motion form of the mechanical arm according to the motion control instruction; if the command is a joint jog control command, the process goes to step S205; if the command is a joint script motion control command, the process goes to step S206; if the command is a cartesian space inching control command, the process goes to step S207; if the command is a cartesian space script motion control command, the process proceeds to step S208.
S205: the motion controller plans a joint motion path according to the joint jog control command, and then proceeds to step S210.
Specifically, the motion controller calculates a joint angle value of the target joint according to a speed set value and an acceleration set value in the joint inching control command, plans an acceleration and constant speed stage of the target joint according to a T curve, and if the inching stopping command is received at the moment, reverses the acceleration and continues to move until the speed is 0.
S206: the motion controller reads and analyzes the joint angle script to obtain the joint motion path, and then proceeds to step S210.
Specifically, the motion controller determines joint angle values in joint motion paths planned by the joint angle script.
S207: the motion controller plans the motion path of the end of the mechanical arm according to the cartesian space jog control command, and then proceeds to step S209.
Specifically, the motion controller calculates the path of the tail end of the mechanical arm in the Cartesian space according to a speed set value and an acceleration set value in the Cartesian space inching control command, plans the acceleration and constant speed stage of the Cartesian space at the tail end of the mechanical arm according to a T curve, and if the inching stopping command is received at the moment, the acceleration is reversed, and the motion is continued until the speed is 0.
S208: the motion controller reads and analyzes the cartesian space script to obtain the motion path of the end of the robot arm, and then proceeds to step S209.
S209: the motion controller uses a reverse solution algorithm of the kinematics of the mechanical arm to solve the joint motion path of the target joint according to the cartesian space coordinates of the motion path of the tail end of the mechanical arm, and then the step S210 is performed.
S210: and the motion controller generates a joint control command according to the joint motion path and sends the joint control command to the corresponding joint controller to control the motion of the mechanical arm.
S211: in the process of controlling the mechanical arm to move, the motion controller judges the content of the received instruction information; if receiving the stress information fed back by the force sensor, entering step S212; if a stop command sent by the upper computer is received, the step S213 is carried out; if the information that all the joint control commands fed back by the joint controller have been executed is received, the process returns to step S203.
S212: the motion controller converts the stress information into Cartesian space coordinate information of the tail end of the mechanical arm, and then adjusts a joint control instruction according to the stress information in a motion compensation mode.
S213: the motion controller controls each joint controller, and then controls the mechanical arm to enter a stop state.
On the basis of the above detailed description of various embodiments corresponding to the robot arm motion control method, the present application also discloses a robot arm motion control system, a robot arm motion control device and a readable storage medium corresponding to the above method.
Fig. 3 is a schematic structural diagram of a robot arm motion control system according to an embodiment of the present disclosure.
As shown in fig. 3, a robot arm motion control system provided in an embodiment of the present application includes: the robot comprises an upper computer 301, a motion controller 302 connected with the upper computer 301, and joint controllers 303 connected with the motion controller 302 and corresponding to all joints of the mechanical arm;
the upper computer 301 is configured to receive a user instruction, generate a motion control instruction according to the user instruction, and send the motion control instruction to the motion controller 302;
the motion controller 302 runs a first asynchronous timer for realizing communication between the motion controller 302 and the upper computer 301, a second asynchronous timer for realizing communication between the motion controller 302 and each joint controller 303 and a third asynchronous timer for executing a motion control program of the mechanical arm, the motion controller 302 is used for receiving a motion control command sent from the upper computer 301, determining a motion path of the mechanical arm according to the motion control command, generating a joint control command for each joint controller 303 according to the motion path, and sending the joint control command to the corresponding joint controller 303;
wherein the first asynchronous timer, the second asynchronous timer and the third asynchronous timer have equal priority.
Further, the motion controller 302 also operates a fourth asynchronous timer for realizing communication between the motion controller 302 and a force sensor provided at the end of the mechanical arm;
correspondingly, the motion controller 302 generates joint control commands for the joint controllers 303 according to the motion path, and specifically includes:
the motion controller 302 compensates the motion path according to the force information of the mechanical arm tail end fed back by the force sensor to obtain a compensated motion path;
the motion controller 302 generates joint control commands for the respective joint controllers 303 based on the compensated motion path;
wherein the fourth asynchronous timer has the same priority as the first asynchronous timer.
Since the embodiment of the system part corresponds to the embodiment of the method part, the embodiment of the system part is described with reference to the embodiment of the method part, and is not repeated here.
Fig. 4 is a schematic structural diagram of a robot arm motion control apparatus according to an embodiment of the present disclosure.
As shown in fig. 4, a robot arm motion control apparatus provided in an embodiment of the present application includes:
a receiving unit 401, configured to receive a motion control instruction sent from an upper computer based on a first asynchronous timer;
a calculating unit 402, configured to determine a motion path of the mechanical arm according to the motion control instruction based on the second asynchronous timer, and generate a joint control instruction of a joint controller corresponding to each joint of the mechanical arm according to the motion path;
a sending unit 403, configured to issue a joint control instruction to a corresponding joint controller based on the third asynchronous timer;
the first asynchronous timer is used for realizing communication between the motion controller and the upper computer, the second asynchronous timer is used for realizing communication between the motion controller and each joint controller, and the third asynchronous timer is used for executing a motion control program of the mechanical arm; the first asynchronous timer, the second asynchronous timer and the third asynchronous timer have equal priority.
Further, the robot arm motion control apparatus provided by the embodiment of the present application may further include:
the monitoring unit is used for monitoring an emergency stop signal;
and the control unit is used for sending an emergency stop control command to the joint controller when receiving the emergency stop signal.
Since the embodiment of the apparatus portion and the embodiment of the method portion correspond to each other, please refer to the description of the embodiment of the method portion for the embodiment of the apparatus portion, and details are not repeated here.
It should be noted that the above-described apparatus embodiments are merely illustrative, and for example, a module may be divided into only one logic function, and may be implemented in other ways, for example, a plurality of modules or components may be combined or integrated into another system, or some features may be omitted or not executed. In addition, the shown or discussed mutual coupling or direct coupling or communication connection may be an indirect coupling or communication connection through some interfaces, devices or modules, and may be in an electrical, mechanical or other form. Modules described as separate parts may or may not be physically separate, and parts displayed as modules may or may not be physical modules, may be located in one place, or may be distributed on a plurality of network modules. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of the present embodiment.
In addition, functional modules in the embodiments of the present application may be integrated into one processing module, or each of the modules may exist alone physically, or two or more modules are integrated into one module. The integrated module can be realized in a hardware mode, and can also be realized in a software functional module mode.
The integrated module, if implemented in the form of a software functional module and sold or used as a separate product, may be stored in a readable storage medium. Based on such understanding, the technical solutions of the present application may be embodied in the form of a software product, which is stored in a storage medium and executes all or part of the steps of the methods described in the embodiments of the present application, or all or part of the technical solutions.
To this end, the present application further provides a readable storage medium, on which a computer program is stored, where the computer program is executed by a processor to implement the steps of the robot arm motion control method.
The readable storage medium may include: various media capable of storing program codes, such as a usb disk, a removable hard disk, a Read-Only Memory ROM (Read-Only Memory), a random Access Memory ram (random Access Memory), a magnetic disk, or an optical disk.
The computer program contained in the readable storage medium provided in this embodiment is capable of implementing the steps of the robot arm motion control method described above when executed by the processor, and the same effect is achieved.
The detailed description is given above on a method, a system, a device and a readable storage medium for controlling the motion of a mechanical arm provided by the present application. The embodiments are described in a progressive manner in the specification, each embodiment focuses on differences from other embodiments, and the same and similar parts among the embodiments are referred to each other. The system, the device and the readable storage medium disclosed by the embodiment correspond to the method disclosed by the embodiment, so that the description is simple, and the relevant points can be referred to the method part for description. It should be noted that, for those skilled in the art, it is possible to make several improvements and modifications to the present application without departing from the principle of the present application, and such improvements and modifications also fall within the scope of the claims of the present application.
It is further noted that, in the present specification, relational terms such as first and second, and the like are used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Also, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising an … …" does not exclude the presence of other identical elements in a process, method, article, or apparatus that comprises the element.
Claims (8)
1. A motion control method of a mechanical arm is characterized in that based on a motion controller, the motion control method comprises the following steps:
receiving a motion control instruction sent by an upper computer;
determining a motion path of the mechanical arm according to the motion control instruction, and generating joint control instructions of joint controllers corresponding to all joints of the mechanical arm according to the motion path;
sending the joint control command to the corresponding joint controller;
the motion controller runs a first asynchronous timer used for realizing communication between the motion controller and the upper computer, a second asynchronous timer used for realizing communication between the motion controller and each joint controller, and a third asynchronous timer used for executing a motion control program of the mechanical arm; the first asynchronous timer, the second asynchronous timer, and the third asynchronous timer have equal priority;
the motion controller also operates a fourth asynchronous timer for realizing communication between the motion controller and a force sensor arranged at the tail end of the mechanical arm;
correspondingly, the generating of the joint control instruction of the joint controller corresponding to each joint of the robot arm according to the motion path specifically includes:
compensating the motion path according to the stress information of the tail end of the mechanical arm fed back by the force sensor to obtain a compensated motion path;
generating joint control instructions for the joint controllers according to the compensated motion path;
wherein the fourth asynchronous timer has equal priority to the first asynchronous timer;
the force sensor is arranged at a clamp used for manual traction at the tail end of the mechanical arm.
2. The robot arm motion control method according to claim 1, further comprising:
monitoring an emergency stop signal;
and when the emergency stop signal is received, sending an emergency stop control command to the joint controller.
3. The robot arm motion control method according to claim 1, wherein a communication protocol between the motion controller and the upper computer is a transmission control protocol; the communication protocol between the motion controller and each joint controller is a control local area network communication protocol; the communication protocol between the motion controller and the force sensor is a user datagram protocol.
4. The method for controlling motion of a robot arm according to claim 1, wherein the determining a motion path of the robot arm based on the motion control command and generating a joint control command for a joint controller corresponding to each joint of the robot arm based on the motion path includes:
calculating to obtain Cartesian space coordinates of the tail end of the mechanical arm according to the joint angle of each joint fed back by each joint controller;
determining the tail end motion path of the tail end of the mechanical arm in a Cartesian space according to the motion control instruction;
obtaining joint movement paths of all joints according to inverse solution of the tail end movement path;
and generating a joint control command for a joint controller of the joint according to the joint motion path of the joint.
5. The robot arm motion control method according to claim 1, wherein the type of the motion control command specifically includes: the control system comprises a joint inching control instruction, a joint script motion control instruction, a Cartesian space inching control instruction, a Cartesian space script motion control instruction and a fixed point motion control instruction.
6. A robot arm motion control system, comprising: the device comprises an upper computer, a motion controller connected with the upper computer, and joint controllers connected with the motion controller and corresponding to all joints of the mechanical arm;
the upper computer is used for receiving a user instruction, generating a motion control instruction according to the user instruction and sending the motion control instruction to the motion controller;
the motion controller runs a first asynchronous timer used for realizing communication between the motion controller and the upper computer, a second asynchronous timer used for realizing communication between the motion controller and each joint controller and a third asynchronous timer used for executing a motion control program of the mechanical arm, and is used for receiving a motion control command sent by the upper computer, determining a motion path of the mechanical arm according to the motion control command, generating a joint control command for each joint controller according to the motion path and sending the joint control command to the corresponding joint controller;
wherein the first asynchronous timer, the second asynchronous timer, and the third asynchronous timer have equal priority;
the motion controller also operates a fourth asynchronous timer for realizing communication between the motion controller and a force sensor arranged at the tail end of the mechanical arm;
correspondingly, the motion controller generates joint control commands for the joint controllers according to the motion path, and specifically includes:
the motion controller compensates the motion path according to the stress information of the tail end of the mechanical arm fed back by the force sensor to obtain a compensated motion path;
the motion controller generates joint control instructions for the joint controllers according to the compensated motion path;
wherein the fourth asynchronous timer has equal priority to the first asynchronous timer;
the force sensor is arranged at a clamp used for manual traction at the tail end of the mechanical arm.
7. A robot arm motion control apparatus, comprising:
the receiving unit is used for receiving a motion control instruction sent by the upper computer based on the first asynchronous timer;
the computing unit is used for determining a motion path of the mechanical arm according to the motion control instruction based on a second asynchronous timer and generating joint control instructions of joint controllers corresponding to joints of the mechanical arm according to the motion path;
the sending unit is used for sending the joint control command to the corresponding joint controller based on a third asynchronous timer;
the first asynchronous timer is used for realizing communication between the motion controller and the upper computer, the second asynchronous timer is used for realizing communication between the motion controller and each joint controller, and the third asynchronous timer is used for executing a motion control program of the mechanical arm; the first asynchronous timer, the second asynchronous timer, and the third asynchronous timer have equal priority;
the motion controller also operates a fourth asynchronous timer for realizing communication between the motion controller and a force sensor arranged at the tail end of the mechanical arm;
correspondingly, the generating of the joint control instruction of the joint controller corresponding to each joint of the robot arm according to the motion path specifically includes:
compensating the motion path according to the stress information of the tail end of the mechanical arm fed back by the force sensor to obtain a compensated motion path;
generating joint control instructions for the joint controllers according to the compensated motion path;
wherein the fourth asynchronous timer has equal priority to the first asynchronous timer;
the force sensor is arranged at a clamp used for manual traction at the tail end of the mechanical arm.
8. A readable storage medium on which a computer program is stored, the computer program, when being executed by a processor, realizing the steps of a robot arm movement control method according to any one of claims 1 to 5.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110210584.5A CN112894827B (en) | 2021-02-25 | 2021-02-25 | Method, system and device for controlling motion of mechanical arm and readable storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110210584.5A CN112894827B (en) | 2021-02-25 | 2021-02-25 | Method, system and device for controlling motion of mechanical arm and readable storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112894827A CN112894827A (en) | 2021-06-04 |
| CN112894827B true CN112894827B (en) | 2022-09-02 |
Family
ID=76107157
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110210584.5A Active CN112894827B (en) | 2021-02-25 | 2021-02-25 | Method, system and device for controlling motion of mechanical arm and readable storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112894827B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114473324B (en) * | 2022-02-21 | 2023-11-03 | 哈尔滨工业大学（深圳） | Multi-mechanical-arm collaborative splicing welding control method and system based on teaching learning |
| CN114932555B (en) * | 2022-06-14 | 2024-01-05 | 如你所视(北京)科技有限公司 | Mechanical arm collaborative operation system and mechanical arm control method |
Family Cites Families (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102195946A (en) * | 2010-03-16 | 2011-09-21 | 上海交技发展股份有限公司 | Intelligent communication controller based on ARM (Advanced RISC Machines) technology |
| CN102455940B (en) * | 2010-10-29 | 2014-02-12 | 迈普通信技术股份有限公司 | Processing method and system of timers and asynchronous events |
| DE102011107169A1 (en) * | 2011-07-13 | 2013-01-17 | Kuka Roboter Gmbh | Control of a robot |
| CN103722565B (en) * | 2014-01-23 | 2015-09-16 | 哈尔滨工业大学 | Anthropomorphic robot self collision monitoring system and method for supervising |
| CN106607901A (en) * | 2015-10-21 | 2017-05-03 | 田荣侠 | Joint controller of three-degree-of-freedom pneumatic series connection mechanical arm |
| CN106272416B (en) * | 2016-08-29 | 2020-12-29 | 上海交通大学 | Robot slender shaft precision assembly system and method based on force sense and vision |
| CN107132814B (en) * | 2017-02-24 | 2019-12-13 | 上海交通大学 | open numerical control system based on machine vision state monitoring and error compensation |
| CN107932504B (en) * | 2017-11-13 | 2020-11-13 | 浙江工业大学 | PyQt-based mechanical arm operation control system |
| CN109249395A (en) * | 2018-10-18 | 2019-01-22 | 清华大学深圳研究生院 | A kind of multi-functional multi-foot robot control system |
| CN110549334A (en) * | 2019-08-15 | 2019-12-10 | 大连大华中天科技有限公司 | Control method of four-degree-of-freedom mechanical arm |
| CN112045677B (en) * | 2020-08-10 | 2024-03-29 | 华东理工大学 | Intelligent reinforcement system for bolt holes of turbine disk of aero-engine and control method of intelligent reinforcement system |
| CN112548527A (en) * | 2020-12-08 | 2021-03-26 | 扬州舞指芯创智能科技有限公司 | Device and method for dynamic shaft hole assembly based on multi-camera visual servo |
-
2021
- 2021-02-25 CN CN202110210584.5A patent/CN112894827B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN112894827A (en) | 2021-06-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US9492925B2 (en) | Robot controller, robot system, robot, robot control method, and program | |
| US9393687B2 (en) | Method for programming an industrial robot and industrial robot | |
| EP3067162B1 (en) | Master-slave system | |
| CN108656112B (en) | Mechanical arm zero-force control experiment system for direct teaching | |
| CN106945043B (en) | Multi-arm cooperative control system of master-slave teleoperation surgical robot | |
| Kröger | Opening the door to new sensor-based robot applications—The Reflexxes Motion Libraries | |
| CN112894827B (en) | Method, system and device for controlling motion of mechanical arm and readable storage medium | |
| Cherubini et al. | Multimodal control for human-robot cooperation | |
| Surdilovic et al. | Development of collaborative robots (cobots) for flexible human-integrated assembly automation | |
| EP3441201B1 (en) | Method for operating a robot and robotic system | |
| CN110709211B (en) | Robot system and control method for robot system | |
| US10625417B2 (en) | Control system, setting device, setting method, and storage device | |
| Decré et al. | Extending the iTaSC constraint-based robot task specification framework to time-independent trajectories and user-configurable task horizons | |
| Hayward et al. | Introduction to RCCL: A robot control &C& library | |
| JP7230128B2 (en) | LEARNING METHOD FOR ROBOT WORK AND ROBOT SYSTEM | |
| Zhou et al. | A bilateral dual-arm teleoperation robot system with a unified control architecture | |
| Selvaggio et al. | Towards a self-collision aware teleoperation framework for compound robots | |
| KR101474778B1 (en) | Control device using motion recognition in artculated robot and method thereof | |
| Liang et al. | Human-robot collaborative semi-autonomous teleoperation with force feedback | |
| Caccavale et al. | RePLiCS: An environment for open real-time control of a dual-arm industrial robotic cell based on RTAI-Linux | |
| Chen et al. | Uncertainty-aware arm-base coordinated grasping strategies for mobile manipulation | |
| JP6668629B2 (en) | Robot controller and robot system | |
| US20230381963A1 (en) | Robot control method, computer-readable storage medium, and robot | |
| KR102624551B1 (en) | A method for task-space compliance control of robot manipulator without rotational displacement constraints | |
| JP7327991B2 (en) | Control method, control program, recording medium, robot system, article manufacturing method, and input device |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |