CN106945044B - Robot pause motion control method and system - Google Patents
Robot pause motion control method and system Download PDFInfo
- Publication number
- CN106945044B CN106945044B CN201710256933.0A CN201710256933A CN106945044B CN 106945044 B CN106945044 B CN 106945044B CN 201710256933 A CN201710256933 A CN 201710256933A CN 106945044 B CN106945044 B CN 106945044B
- Authority
- CN
- China
- Prior art keywords
- motion
- current
- target
- mechanical arm
- track
- 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
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/1602—Programme controls characterised by the control system, structure, architecture
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J13/00—Controls for manipulators
- B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Human Computer Interaction (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
Abstract
the invention relates to a robot pause motion control method and system, wherein the method comprises the following steps: receiving a pause motion instruction transmitted by a control end; when the current robot is in any one current motion state of point-to-point motion, linear motion, circular motion or motion returning to the original point; acquiring a target point of current motion and the residual time of the motion, and storing the target point and the residual time of the motion as track calculation parameters for restoring the current motion; reading the current position, the current speed and the current acceleration of the tail end of the mechanical arm, and calculating a pause motion track which is passed by the tail end of the mechanical arm when the tail end of the mechanical arm is changed from the current position, the current speed and the current acceleration to stop; calculating a target angle, a target angular velocity and a target angular acceleration of a mechanical arm joint at each position when the tail end of the mechanical arm runs on the pause motion track, and forwarding the target angle, the target angular velocity and the target angular acceleration to the control master station; the invention can form a complete robot pause motion control system, reduce the development cost of the robot control system and improve the control effect.
Description
Technical Field
The invention relates to the technical field of robot control, in particular to a robot pause motion control method and system.
Background
the Robot Operating System (ROS) is an open-source Robot Operating System, and can provide a standardized and open-source programming framework for Robot developers. ROS currently do not support real-time threading. Open robot control Software (OROCOS) is also an Open-source robot control Software programming framework, and is characterized by supporting real-time thread operation, but the openness and the universality of the framework are not good as those of an ROS (reactive oxygen species).
therefore, the robot pause motion is an important motion mode of the robot, and in some application schemes at present, the OROCOS is adopted on the ROS, but in the existing technical scheme, a complete robot controller software cannot be formed on a built framework, the characteristics of the ROS and the OROCOS cannot be fully utilized when the pause motion is executed, the system development cost is high, and the control effect is poor.
disclosure of Invention
therefore, in order to solve the above technical problems, it is necessary to provide a method for controlling a robot pause motion, which reduces the system development cost and improves the control effect.
a robot pause motion control method comprising:
receiving a pause motion instruction transmitted by a control end;
Executing the pause motion instruction when the current robot is in any one current motion state of point-to-point motion, linear motion, circular motion or motion returning to the original point;
Acquiring a target point of current movement and the movement residual time, and storing the target point and the movement residual time as track calculation parameters for restoring the current movement; reading the current position, the current speed and the current acceleration of the tail end of the mechanical arm, and calculating a pause motion track which is passed by the tail end of the mechanical arm when the tail end of the mechanical arm is changed from the current position, the current speed and the current acceleration to stop;
and calculating the target angle, the target angular velocity and the target angular acceleration of the mechanical arm joint at each position when the tail end of the mechanical arm runs on the pause motion track, and forwarding the target angle, the target angular velocity and the target angular acceleration to the control master station.
a robot pause motion control system comprising: the system comprises a master control module, an algorithm module and a communication management module;
the master control module is used for receiving a motion pause instruction transmitted by the control end; executing the pause motion instruction when the current robot is in any one current motion state of point-to-point motion, linear motion, circular motion or motion returning to the original point;
The algorithm module is used for acquiring a target point of current motion and the residual time of the motion, and storing the target point and the residual time of the motion as track calculation parameters for restoring the current motion; reading the current position, the current speed and the current acceleration of the tail end of the mechanical arm, and calculating a pause motion track which is passed by the tail end of the mechanical arm when the tail end of the mechanical arm is changed from the current position, the current speed and the current acceleration to stop; calculating the target angle, the target angular velocity and the target angular acceleration of the mechanical arm joint at each position when the tail end of the mechanical arm runs on the pause motion track;
and the communication management module is used for forwarding the target angle, the target angular velocity and the target angular acceleration to the control master station.
After a pause motion instruction transmitted by a control end is received, calculating a motion track of pause motion when the current robot is in any one current motion state of point-to-point motion, linear motion, circular motion or motion returning to an original point, calculating state parameters such as a target angle, a target angular velocity and a target angular acceleration of each joint of a mechanical arm running on the motion track in real time, and forwarding the state parameters to a control master station; realizing the control process of the pause motion of the robot; the technical scheme can form a complete robot pause motion control system, reduce the development cost of the robot control system and improve the control effect.
drawings
FIG. 1 is a flow chart of a robot pause motion control method according to an embodiment of the invention;
FIG. 2 is a schematic diagram of a robot pause motion control system;
FIG. 3 is a flow chart of the algorithm executed by the general control module;
FIG. 4 is a flow chart of an algorithm module executing an algorithm;
FIG. 5 is a hardware architecture model of a robotic control system of an application example;
FIG. 6 is a diagram of a software architecture based on the building of ROS and OROCOS;
FIG. 7 is a state change schematic of the controller state machine;
Fig. 8 is a state change diagram of a device state machine.
Detailed Description
the following describes an embodiment of the robot pause motion control method according to the present invention with reference to the drawings.
in the embodiment of the invention, the motion pause means that the mechanical arm pauses the current motion instruction, the speed is reduced to 0, and after the recovery instruction is received, the mechanical arm continues to move along the currently planned track until the current instruction target position is reached.
Referring to fig. 1, fig. 1 is a flowchart of a robot pause motion control method according to an embodiment of the present invention, including:
s10, receiving a pause motion command transmitted by the control end;
In the above steps, a pause motion instruction may be received by using a preset communication protocol and in an asynchronous remote procedure call manner;
in the process, an operator can generate a pause motion instruction through a human-computer interaction interface of the control end, and the instruction does not need to transmit parameters; the pause motion command is received from The human-computer interaction interface in an asynchronous remote procedure call mode through a preset communication protocol, such as a communication protocol developed based on IEC (The Internet communication engine).
S20, executing the pause motion command when the current robot is in any current motion state of point-to-point motion, linear motion, circular motion or motion back to the original point;
in the step, whether the current robot motion state is a point-to-point motion state, a linear motion state, an arc motion state or a return-to-original-point motion state capable of executing pause motion is judged, and if yes, the pause motion instruction is executed;
in one embodiment, the state of the controller state machine can be monitored in real time, and when the controller state machine is switched to the state of executing the pause motion, a pause motion planning function is called to start executing the sudden stop motion planning process;
for judging whether the current robot motion state is a specified motion state capable of executing pause motion or not, judging whether the controller state machine is one of four states of point-to-point motion, linear motion, circular motion and motion returning to an original point or not, if so, executing a step of calling a pause motion planning function, and otherwise, refusing to execute the pause operation instruction.
s30, acquiring a target point of the current movement and the movement residual time, and storing the target point and the movement residual time as track calculation parameters for restoring the current movement; reading the current position, the current speed and the current acceleration of the tail end of the mechanical arm, and calculating a pause motion track which is passed by the tail end of the mechanical arm when the tail end of the mechanical arm is changed from the current position, the current speed and the current acceleration to stop;
In one embodiment, after the pause motion instruction is executed, a pause motion execution function may be asynchronously triggered and called through the first interface; wherein the first interface is a real-time input/output interface based on creating OROCOS on ROS;
Further, the pause motion planning function may be called by the Operational Caller method of OROCOS;
executing the pause motion planning function, acquiring and recording a target point of current motion and the residual time of the motion, reading the current position, the current speed and the current acceleration of the tail end of the mechanical arm, and calculating a pause motion track according to the current position, the current speed and the current acceleration;
In one embodiment, executing the pause motion planning function transitions the controller state machine to an execute pause motion (active.
as an embodiment, if the current motion is a point-to-point motion or a return-to-origin motion, the method for calculating the pause motion trajectory may include the following steps:
(A1) Calculating a target point moving from a current point to a point or returning to an original point, calculating the remaining time of the current movement according to the total time length of the current movement and the elapsed time of the current movement, and recording the target point and the remaining time;
the current point-to-point or the target point theta of the return-to-origin motion1the remaining time of the current motion: tr ═ T0-t0wherein T is0is the total duration of the current movement, t0Is the time the current motion has elapsed;
(A2) Reading the current joint angle, joint angular velocity and joint angular acceleration of the mechanical arm;
Specifically, the current joint angle theta of the mechanical arm is read0Angular velocity of jointangular acceleration of joint
(A3) Recording the current time as 0, calculating the time of reducing the speed of each joint to 0 according to the principle that the maximum acceleration reduces the speed to 0, and recording the time as the execution time of the pause operation instruction;
the time calculation formula for the velocity drop of each joint to be 0 is as follows:
wherein i represents the joint number, T is selectedithe maximum value of the operation instruction is recorded as a pause operation instruction execution time T;
(A4) And obtaining a pause running track according to the pause running instruction execution time T.
In one embodiment, if the current motion is a linear motion, the method for calculating the pause motion trajectory may include the following steps:
(B1) Calculating a target point of the current linear motion, calculating the remaining time of the current motion according to the total time length of the current motion and the elapsed time of the current motion, and recording the target point and the remaining time;
Specifically, the target point s of the current linear motioneThe remaining time of the current motion: tr ═ T0-t0wherein, T0Is the total duration of the current movement, t0Is the time that the current motion has elapsed.
(B2) Calculating the track, the joint angular velocity and the joint angular acceleration of the mechanical arm at the current moment according to the linear motion track;
Specifically, the current linear motion track is planned by a known algorithm, so that the track s of the mechanical arm at the current moment can be calculated0Angular velocity of jointangular acceleration of joint
(B3) calculating a track which needs to be walked by the suspended linear motion according to the track, the joint angular velocity and the joint angular acceleration to obtain a suspended motion track, and recording the track which is walked by the mechanical arm along the straight line when the motion is recovered;
Specifically, the calculation formula of the track that needs to be walked when the linear motion is suspended is as follows:
wherein, amaxthe maximum acceleration of the linear motion; record s2＝s0+s1wherein s is2to restore the trajectory the arm has followed in a straight line during the movement.
in one embodiment, if the current motion is a circular motion, the method for calculating the pause motion trajectory may include the following steps:
(C1) calculating a target point of the current circular motion, calculating the remaining time of the current motion according to the total duration of the current motion and the elapsed time of the current motion, and recording the target point and the remaining time;
Target point s of the current circular motioneThe remaining time of the current motion: tr ═ T0-t0wherein, T0is the total duration of the current movement, t0is the time the current motion has elapsed;
(C2) calculating the current track, joint angular velocity and joint angular acceleration of the mechanical arm according to the circular arc motion track;
specifically, the current circular motion track is planned by a known algorithm, so that the track s of the mechanical arm at the current moment can be calculated0angular velocity of jointAngular acceleration of joint
(C3) Calculating a track which needs to be walked by the suspended circular motion according to the track, the joint angular velocity and the joint angular acceleration to obtain a suspended running track, and recording the track which is walked by the mechanical arm along the circular arc when the motion is recovered;
the calculation formula of the track which needs to be walked when the circular motion is paused is as follows:
wherein, amaxThe maximum acceleration of the circular motion; record s2＝s0+s1Wherein s is2In order to restore the track that the mechanical arm has travelled along the arc during the movement.
and S40, calculating the target angle, the target angular velocity and the target angular acceleration of the mechanical arm joint at each position when the tail end of the mechanical arm runs on the pause motion track, and forwarding the target angle, the target angular velocity and the target angular acceleration to the control main station.
in one embodiment, the target angle, the target angular velocity and the target angular acceleration of the mechanical arm joint are sent to equipment communication software through a second interface and forwarded to a control master station; wherein the second interface is a real-time input/output interface based on creating OROCOS on ROS.
as an embodiment, a method for calculating a target angle, a target angular velocity, and a target angular acceleration of a robot joint at each position of the robot joint when the robot arm tip is running on the pause motion trajectory may include the steps of:
(a) Counting the motion time t of the mechanical arm after the mechanical arm starts to move on the pause motion trackn；
(b) If the movement time satisfies: t is tnCalculating the target angle, the target angular velocity and the target angular acceleration of each joint of the mechanical arm at intervals of set time tau; τ is 1/f, f denotes refresh frequency; typically, τ is 1 millisecond;
(c) If the movement time tnsatisfies the following conditions: t is tnIf the distance is more than T, stopping moving; wherein T is the pause instruction execution time.
as in the above embodiment, if the current motion is a point-to-point motion or a return-to-origin motion, the method for calculating the target angle, the target angular velocity and the target angular acceleration of each joint of the mechanical arm includes the following steps:
(A5) calculating the target angular acceleration of each mechanical arm joint according to the current joint angular velocity of the mechanical arm and the execution time of the pause operation instruction;
Specifically, the target angular acceleration of the mechanical arm jointthe calculation formula is as follows:
(A6) calculating the target angular velocity and the joint target angle of each mechanical arm joint according to the target angular acceleration;
specifically, the target angular velocity of each mechanical arm jointtarget angle theta with jointiThe calculation formulas of (A) and (B) are respectively as follows:
In the formula, theta0is the current joint angle of the mechanical arm,in order to determine the angular velocity of the joint,is the angular acceleration of the joint; t denotes the moment during the pause movement.
as in the above embodiment, if the current motion is a linear motion, the method for calculating the target angle, the target angular velocity, and the target angular acceleration of each joint of the robot arm includes the following steps:
(B4) Calculating the track position, the track speed and the track acceleration of the mechanical arm at any moment t in the pause motion:
Specifically, the track position, the track speed, and the track acceleration at any time t are:
wherein s is0Is the track of the mechanical arm at the current moment,In order to determine the angular velocity of the joint,is the angular acceleration of the joint, s is the track position,In order to be the speed of the track,Is the track acceleration;
(B5) calculating the target position, speed and acceleration of the tail end of the mechanical arm according to the track position, the track speed and the track acceleration;
specifically, the calculation formula of the target position, the speed and the acceleration of the tail end of the mechanical arm is as follows:
wherein, Ptis the position of the target, and is,In order to be the speed of the vehicle,In order to be able to accelerate the vehicle,is the unit vector of the current linear motion;
(B6) Calculating a target angle, a target angular velocity and a target angular acceleration of a mechanical arm joint according to the target position, the velocity and the acceleration by using an inverse kinematics method;
specifically, the calculation formula of the target angle, the target angular velocity and the target angular acceleration of the mechanical arm joint is as follows:
θt＝invKinematics(Pt)
wherein, thetatIn order to obtain the target angle,in order to obtain the target angular velocity,for the target angular acceleration, invKinematics () represents a calculation formula of inverse kinematics, J is a jacobian matrix,Is a first derivative of the Jacobian matrix, J+is the pseudo-inverse of.
As in the above embodiment, if the current motion is a circular arc motion, the method for calculating the target angle, the target angular velocity, and the target angular acceleration of each joint of the mechanical arm includes the following steps:
(C4) calculating the track position, the track speed and the track acceleration of the mechanical arm at any moment t in the pause motion;
Specifically, the track position, the track speed, and the track acceleration at any time t are:
Wherein s is0is the track of the mechanical arm at the current moment,in order to determine the angular velocity of the joint,Is the angular acceleration of the joint, s is the track position,in order to be the speed of the track,Is the track acceleration;
(C5) Calculating the target position, speed and acceleration of the tail end of the mechanical arm according to the track position, the track speed and the track acceleration;
Specifically, the target position P of the end of the robot armt＝[xt,yt,zt]T：
Wherein, thetatis a circular arc P0Ptcorresponding central angle of the circlea1,a2,a3,b1,b2,b3The parameters such as R are all known parameters of the current circular arc motion track;
target velocity of end of mechanical armand accelerationthe calculation formula is as follows:
wherein the content of the first and second substances,is a point PtA tangent vector of;
(C6) and calculating a target angle, a target angular velocity and a target angular acceleration of the mechanical arm joint according to the target position, the velocity and the acceleration by using an inverse kinematics method.
The calculation formula of the target angle, the target angular velocity and the target angular acceleration of the mechanical arm joint is as follows:
θt＝invKinematics(Pt)
Wherein, thetatIn order to obtain the target angle,in order to obtain the target angular velocity,For the target angular acceleration, invKinematics () represents a calculation formula of inverse kinematics, J is a jacobian matrix,Is a first derivative of the Jacobian matrix, J+is the pseudo-inverse of.
According to the technical scheme of the embodiment, a pause motion instruction is received in an asynchronous remote process calling mode by utilizing a real-time input/output interface of an OROCOS (object optical control System), a pause motion instruction trigger function is started, a pause motion execution function of robot motion planning is called, a motion track of pause motion is calculated according to the function, state parameters such as a target angle, a target angular velocity and a target angular acceleration of each joint of a mechanical arm running on the motion track are calculated in real time, and the state parameters are forwarded to a control master station through equipment communication software, so that a pause motion control process of the robot is realized; the technical scheme can perform the functions of receiving and analyzing control instructions, calling and executing algorithms, calculating and transmitting parameters in real time and the like, can form a complete robot pause motion control system, reduces the development cost of the robot control system and improves the control effect; in addition, the application of a controller state machine is combined, the optimization control of the algorithm processing process is realized, and the control effect is further improved.
aiming at the robot pause motion control method, the invention provides a robot pause motion control system corresponding to the control method,
Referring to fig. 2, fig. 2 is a schematic structural diagram of a robot pause motion control system, which includes: the system comprises a master control module, an algorithm module and a communication management module;
The master control module is used for receiving a motion pause instruction transmitted by the control end; executing the pause motion instruction when the current robot is in any one current motion state of point-to-point motion, linear motion, circular motion or motion returning to the original point;
The algorithm module is used for acquiring a target point of current motion and the residual time of the motion, and storing the target point and the residual time of the motion as track calculation parameters for restoring the current motion; reading the current position, the current speed and the current acceleration of the tail end of the mechanical arm, and calculating a pause motion track which is passed by the tail end of the mechanical arm when the tail end of the mechanical arm is changed from the current position, the current speed and the current acceleration to stop; calculating the target angle, the target angular velocity and the target angular acceleration of the mechanical arm joint at each position when the tail end of the mechanical arm runs on the pause motion track;
And the communication management module is used for forwarding the target angle, the target angular velocity and the target angular acceleration to the control master station.
in one embodiment, the master control module is in communication connection with the algorithm module through a first interface, the algorithm module is in communication connection with the communication management module through a second interface, and the first interface and the second interface are real-time input/output interfaces for creating an OROCOS on the basis of an ROS;
After the master control module executes the pause motion instruction, asynchronously triggering a pause motion execution function and calling a pause motion planning function through a first interface;
The algorithm module executes the pause motion planning function and calculates a pause motion track;
and sending the target angle, the target angular velocity and the target angular acceleration of the mechanical arm joint to a communication management module through a second interface.
According to the technical scheme of the embodiment, a real-time input/output interface of an OROCOS is utilized, a master control module receives a pause motion instruction in an asynchronous remote process call mode through a set communication protocol, starts a pause motion instruction trigger function, calls a pause motion execution function of a robot motion planning of an algorithm module, calculates a motion track of pause motion according to the function, calculates state parameters such as a target angle, a target angular velocity and a target angular acceleration of each joint of a mechanical arm running on the motion track in real time, and transmits the state parameters to a control master station to realize a pause motion control process of the robot; the technical scheme can perform the functions of receiving and analyzing control instructions, calling and executing algorithms, calculating and transmitting parameters in real time and the like, can form a complete robot pause motion control system, reduces the development cost of the robot control system and improves the control effect.
As an embodiment, the general control module executes an algorithm flow, which can be referred to as fig. 3, where fig. 3 is a flowchart of the general control module executing the algorithm; the method comprises the following specific steps:
1) And generating a pause motion instruction by an operator through a man-machine interaction interface.
2) And the master control module receives a pause motion instruction from the man-machine interaction interface in an asynchronous remote process call mode through a communication protocol.
3) after the command reaches the master control module, the asynchronous trigger suspends the motion execution function, and the function judges whether the controller state machine is in one of four states of point-to-point motion, linear motion, circular motion and return to the original point. If not, the execution of the pause operation instruction is refused.
4) If the controller state machine is one of four states of point-to-point motion, linear motion, circular motion and returning to the origin, the following operations are executed:
a) The pause motion planning function of the algorithm module is called by the Operational Caller method of OROCOS.
b) The controller state machine is transitioned to an execute pause motion (active.
as an example, the algorithm module executes the algorithm flow, which can be referred to fig. 4, where fig. 4 is a flowchart of the algorithm module executing the algorithm; the method comprises the following specific steps:
1) and calling the pause motion planning function by the master control module to start executing a pause motion planning process.
2) If the current motion is a return-to-zero motion or a point-to-point motion, calling a point-to-point pause planning algorithm:
a) Recording the target point theta of the current movement1；
b) Recording the remaining time of the current movement: tr is T-T. Wherein T is the total duration of the current movement, and T is the elapsed time of the current movement;
c) reading current joint angle theta of mechanical arm0Angular velocity of jointangular acceleration of joint
d) recording the current time as 0, and calculating the time for reducing the velocity of each joint to 0 according to the principle that the maximum acceleration reduces the velocity to 0:
wherein i represents the number of joints. Selecting TiThe maximum value of (1) is recorded as pause instruction execution time and is recorded as T.
e) the acceleration of each joint is calculated as:
f) the velocity and joint angle positions corresponding to the time t are respectively:
3) If the current motion is linear motion, calling a linear pause algorithm:
a) recording the current movement of the target point se。
b) recording the remaining time of the current movement: tr is T-T. Wherein T is the total duration of the current movement, and T is the elapsed time of the current movement;
c) because the current linear motion track can be planned by a quintic polynomial algorithm, the track s of the mechanical arm at the current moment can be calculated0Angular velocity of jointAngular acceleration of joint
d) Calculating the track which needs to be walked by the linear motion pause:
wherein, amaxThe maximum acceleration of the linear motion.
e) recording the track that the mechanical arm has travelled along a straight line when the motion is restored: s2＝s0+s1；
f) calculating the track, the track speed and the track acceleration of the mechanical arm at any moment t in the pause motion:
g) calculating the target position, speed and acceleration of the tail end of the mechanical arm:
Wherein the content of the first and second substances,Is a unit vector of linear motion.
h) Using an inverse kinematics method, the position of the joint angle is calculated:
θt＝invKinematics(Pt) (8)
i) the angular velocity and the angular acceleration of the joint can be obtained by the following expressions:
wherein J is a Jacobian (Jacobian) matrix,Is a first derivative of the Jacobian matrix, J+is the pseudo-inverse of (pseudoverse).
4) if the current motion is circular motion, calling an algorithm of circular pause:
a) recording the current moving target track se；
b) recording the remaining time of the current movement: tr is T-T. Wherein T is the total duration of the current movement, and T is the elapsed time of the current movement;
c) because the current circular motion track can be planned by a quintic polynomial algorithm, the track s of the mechanical arm at the current moment can be calculated0Angular velocity of jointAngular acceleration of joint
d) Calculating the track which needs to be walked when the circular motion is paused:
wherein, amaxThe maximum acceleration of the linear motion.
e) Recording the track that the mechanical arm has traveled along the arc when the motion is recovered: s2＝s0+s1
f) Calculating the track, the track speed and the track acceleration of the mechanical arm at any moment t in the pause motion:
g) calculating a target position P of the tipt＝[xt,yt,zt]T：
Wherein, thetatIs a circular arc P0Ptcorresponding central angle of the circlea1,a2,a3,b1,b2,b3the parameters such as R can be obtained by a circular motion quintic polynomial planning algorithm;
h) Determining the target speed of the endand acceleration
wherein the content of the first and second substances,is a point PtA tangent vector of;
i) Using an inverse kinematics method, the position of the joint angle is calculated:
θt＝invKinematics(Pt) (16)
wherein invKinematics () represents a calculation formula of inverse kinematics.
j) the angular velocity and the angular acceleration of the joint can be obtained by the following expressions:
Wherein J is a Jacobian (Jacobian) matrix,is a first derivative of the Jacobian matrix, J+is the pseudo-inverse of (pseudoverse).
5) Time of movement tnnot exceeding the movement time length T:
a) Respectively calculating the target angle, the target angular velocity and the target angular acceleration of each joint of the mechanical arm every 1 millisecond;
b) and sending the target angle, the target angular velocity and the target angular acceleration of each joint of the mechanical arm to a communication management module.
6) time of movement tnAnd stopping the movement when the movement time length T is exceeded.
as an embodiment, the communication management module may be further configured to execute the following algorithm:
Reading state information of a robot motor, calculating the state information of a robot mechanical arm according to a robot motion model, feeding back the state information of the robot to a master control module through a third interface, and feeding back the state information of the robot to an algorithm module through a second interface; the state information of the motor comprises position, speed, moment and the like; the state information of the mechanical arm comprises joint angles, joint angular velocities, joint angular accelerations, tail end poses, tail end linear velocities, tail end angular velocities, tail end linear accelerations and the like.
in order to make the technical solution of the embodiment of the present invention clearer, the following explains the application example of the hardware and software environment using the present invention:
Referring to fig. 5, fig. 5 is a hardware structure model of a robot control system of an application example, a built software architecture of the robot controller runs on a Linux operating system, the Linux host may be a PC with an X86 architecture, or a development board with an ARM chip embedded architecture, and the master control module accesses a control instruction of a human-computer interaction interface of a control end.
The Linux host can install the following software: installing a real-time kernel patch of Xenomai, RTAI or RT Preempt; and installing ROS, OROCOS, rFSM and other software.
Referring to FIG. 6, FIG. 6 is a diagram of a software architecture based on the building of ROS and OROCOS; in the control process, the operating system runs a master control module, an algorithm module and a communication management module.
1. for the master control module:
(1) the master control module creates a Package of the ROS, denoted as Ec _ control _ system, using the method of orocure-catkin-pkg of the ROS, and then in the Package, by inheriting the RTT of the OROCOS: : the TaskContext class is denoted as Ec _ control _ system _ component.
in the constructor of the Ec _ control _ system _ component class, it is arranged to perform the following operations:
a) RTT with OROCOS: : input and RTT: : the Output method defines the input and Output interfaces of the module.
wherein the input interface comprises:
firstly, diagnostic data are transmitted by a communication management module;
State feedback information transmitted by the communication management module: including motor operating conditions, etc.;
State of controller state machine;
The interface of the output includes:
and (4) triggering the event of the controller state machine and outputting the event to the controller state machine.
b) The function call interface is set using the Operational Call method of OROCOS.
Firstly, a callback function of an event report is set: and responding to the event report processing request, wherein the event report processing request comprises information such as error time stamp, event level and the like, and sending the event information to a human-computer interaction interface for displaying.
secondly, setting a callback function of alarm setting: and judging whether to generate an alarm or not according to the diagnosis information. Such as position, velocity, whether acceleration is overrun, etc.
thirdly, control instruction trigger functions of various motion plans are set, and the functions call corresponding response functions of the algorithm module.
c) And calling a Properties method of the OROCOS to define the attributes of the master control module, and defining the attributes of the number of the mechanical arm joints by the master control module.
(2) in the StartHook () member function of Ec _ control _ system _ component, it is set to perform the following operations:
a) checking whether the log report is normal or not, if so, directly exiting, and transmitting relevant information to a master control module for processing through an event report interface;
b) establishing communication connection with a human-computer interaction interface through a communication protocol developed by an ICE, calling a dynamic asynchronous Remote Procedure Call (RPC) method provided by the communication protocol, and binding a callback function responding to a control instruction initiated by the human-computer interaction interface. The callback function firstly judges a calling type according to a first parameter transmitted by a remote process asynchronous calling method provided by ICE (Internet Communications Engine), and then selects and calls a motion instruction trigger function of a corresponding motion plan according to The type.
(3) for the clearuphook () member function of the Ec _ control _ system _ component, in order to enable the function to implement automatic call when the master control module finishes running, the following operations may be further configured to be performed:
a) And calling a communication protocol interface developed by the ICE, and closing the communication connection with the human-computer interaction interface.
(4) For the controller state machine, referring to fig. 7, fig. 7 is a schematic diagram of a state change of the controller state machine; eleven states of Init, Ready, Fault, active.recovery, active.halt, active.handles, active.Tozero, active.PTP, active.Line, active.circle and active.stop can be set, and respectively represent initialization, instruction input waiting, recovery, pause, manual teaching, origin returning, pause motion and emergency stop state. The Active state transition rule is effective to eight sub-states, wherein eight states of Active, recovery, Active, halt, Active, Tozero, Active, PTP, Active, line, Active, circle and Active, stop form a set of Active states. For example, writing an "e _ Ready" event to any of the eight states transitions the state of the controller state machine from the current state to the Ready state (i.e., the wait for instruction input state).
in addition, the Lua language can also be used for writing a start file of the master control module, and the start file is set to execute the following actions:
a) Loading a module to run by using an import method of the OROCOS;
b) Defining the refreshing frequency of a module and the priority level of a thread;
c) Assigning the attribute of the module;
d) and establishing connection between the input and output interfaces of the master control module and the interfaces of the algorithm module and the communication management module by using a connect method of the OROCOS.
e) And operating the master control module by using a start method of the OROCOS, wherein the master control module calls the Starthook () function firstly and then periodically calls the Updatehook () function in real time according to a preset refresh frequency.
2. For the algorithm module:
the algorithm module creates a Package of ROS, denoted as Ec _ control _ loop, using the method of orocure-catkin-pkg of ROS, and then in the Package, by inheriting the RTT of OROCOS: : the TaskContext class is denoted as Ec _ control _ loop _ component.
(1) in the constructor of the Ec _ control _ loop _ component class, it is arranged to perform the following operations:
a) The algorithm module uses the RTT of OROCOS: : input and RTT: : the Output method defines input and Output interfaces.
Wherein the input interface comprises:
Firstly, motor operation data are transmitted by a communication management module;
Diagnostic data transmitted by the communication management module;
State of the equipment state machine;
The state of the controller state machine;
The interface of the output includes:
firstly, outputting motor control instruction data to an equipment communication module;
triggering an event of the equipment state machine and outputting the event to the equipment state machine;
triggering the controller state machine event and outputting the event to the controller state machine.
b) the method comprises the following steps of setting a function call interface by using an Operational Call method of OROCOS, and setting an interface of an event report: the interface triggers an event report processing function of the master control module to set return origin instruction response functions of various motion plans.
c) And calling the Properties method of the OROCOS to define the attribute of the algorithm module, wherein the algorithm module defines the attribute of the number of the mechanical arm joints.
(2) In the StartHook () member function of Ec _ control _ loop _ component, it is set to perform the following operations:
a) checking whether the log report is normal or not, if so, directly exiting, and transmitting relevant information to a master control module for processing through an event report interface;
b) and checking whether the motor operation data channel has data or not, if not, directly exiting, and transmitting the related information to the master control module for processing through the event report interface.
(3) for the UpdateHook () member function of the Ec _ control _ loop _ component class, setting the function to run in real time according to the frequency set by the user (for example, setting to 100Hz) when the algorithm module runs, may be set to perform the following operations:
a) reading a controller state machine state;
b) according to different states of the controller state machine, different operations are executed:
And I, if the motion is the pause motion, pausing the motion, manually teaching, scramming, and returning to the original point state. At this time, the following operations are performed:
if the number of the control instructions in the instruction buffer area is less than 20, all the instructions are sent to the communication management module together, and the state of the controller state machine is changed into a waiting instruction input state;
if the number of the control instructions in the instruction buffer area is more than 20, taking 20 control instructions at the tail of the instruction queue and sending the control instructions to the communication management module;
and II, if the state is the pause state, nothing is done.
(4) For the Ec _ control _ loop _ component class, function call interfaces of pause motion, manual teaching, scram, returning to the origin, pause, recovery and the like are defined, and the following are realized:
a) pause motion, manual teaching, function of returning to the origin, the internal implementation is as follows:
It is checked whether the controller state machine is in a wait for instruction input state. If not, quitting and transmitting the related information to the master control module for processing through the event report interface;
Reading current state information of the motor;
Respectively calling pause motion, manual teaching and motion planning returning to the original point according to the current state of the motor, and storing the generated motor control command in a command buffer area;
the controller state machine is set to the corresponding state. Such as pausing a motion callback function, the controller state machine is set to the pause motion state.
b) the pause function, implemented internally as follows:
Checking whether the controller state machine is in a state of pause motion, manual teaching, returning to an original point and the like. If not, quitting and transmitting the related information to the master control module through the event report interface for processing;
recording the current state of the current controller state machine, and converting the state of the controller state machine into a pause state.
c) the recovery function, implemented internally, is as follows:
It is checked whether the controller state machine is in a suspended state. If not, quitting and transmitting the related information to the master control module for processing through the event report interface;
The state of the controller state machine is transitioned to the state before the pause.
d) the scram function, implemented internally as follows:
and I, checking whether the controller state machine is in a state of pause motion, manual teaching, return to an original point and the like. If not, quitting and transmitting the related information to the master control module for processing through the event report interface;
II, reading the current state information of the motor;
III, clearing the motor control instruction buffer zone;
and IV, calling a speed planning motion plan to reduce the speed of the motor to 0 in the shortest time, and storing the generated motor control command in a command buffer area.
(5) Writing a starting file of an algorithm module by using a Lua language, and setting to execute the following actions:
a) loading an algorithm module by using an import method of OROCOS;
d) defining the refreshing frequency of an algorithm module and the priority level of a thread;
c) assigning the attribute of the algorithm module;
d) and establishing connection between the input and output interfaces of the algorithm module and the interfaces of the master control module and the communication management module by using a connect method of the OROCOS.
e) And operating an algorithm module by using a start method of the OROCOS, wherein the algorithm module calls a Starthook () function at first and then periodically calls an Updatehook () function in real time according to the set refresh frequency.
3. For the communication management module:
The communication management module can communicate with the Arm development board through ttyACM0 in linux host minicom, and a CANOpen master station protocol can run on the Arm development board, and the master station protocol can set an instruction buffer area and can store 25 instructions at most.
the communication management module may utilize the RTT of OROCOS: : input and RTT: : the Output method is communicated with the robot algorithm module and the master control module.
And establishing a device state machine by using rFSM software to control the service logic of the communication management module.
the communication management module utilizes RTT of OROCOS: : input and RTT: : the Output method is connected with the equipment state machine, can change the state of the equipment state machine, and reads the state.
(1) the communication module creates a Package of ROS using the method of orocure-catkin-pkg of ROS, and then in the Package, by inheriting the RTT of OROCOS: : the TaskContext class creates a real-time module of OROCOS, denoted as Ec _ component.
in the constructor of the Ec _ component class, the following operations are set to be performed:
a) the communication management module utilizes RTT of OROCOS: : input and RTT: : the Output method defines input and Output interfaces.
Wherein the input interface comprises:
firstly, control instruction data transmitted by an algorithm module;
State of the equipment state machine;
The interface of the output includes:
Firstly, diagnosis data are output to an algorithm module and a master control module;
Secondly, outputting motor operation data and mechanical arm state data to an algorithm module;
triggering the event of the state machine and outputting the event to the equipment state machine.
b) and defining a function calling interface by using an Operational Call method of the OROCOS, defining an interface of the event report by using the communication management module, and triggering an event report processing function of the master control module through the interface.
c) and calling the Properties method of the OROCOS to define the attribute of the communication management module, wherein the communication management module defines the attribute of the number of the mechanical arm joints.
(2) in the StartHook () member function of Ec _ component, it is set to perform the following operations:
a) checking whether the log report is normal or not, if so, directly exiting, and transmitting relevant information to the master control module through the event report interface for processing;
b) Initializing motor driving:
i, establishing communication with a motor driver through ttyACM 0;
II, emptying a motor instruction buffer queue;
Enabling the motor, if the enabling is successful, carrying out the next step, otherwise, exiting;
IV, reading the position of a motor, and calculating the current state of a mechanical arm of the robot, including a joint angle and the terminal pose of the mechanical arm;
c) Initializing the state of the mechanical arm:
and judging whether the mechanical arm needs to execute zero returning movement or not according to the position of the motor. And if the difference between any joint angle of the mechanical arm and the zero degree is more than 0.01 degree, executing zero returning motion, calling pause motion planning, and planning the zero returning motion.
d) Changing the state of the device state machine:
If the mechanical arm of the robot needs to execute zero returning movement, controlling the equipment state machine to keep the Init state unchanged; otherwise, sending an "e _ nominal" event to the device state machine, and converting the state of the device state machine into active.
(3) for the UpdateHook () member function of Ec _ component, the function is set to run in real time (for example, set to 1KHz) at a frequency set by a user when the communication management module runs, and is set to perform the following operations:
a) reading the state of the device state machine;
b) according to different states of the equipment state machine, different operations are executed:
and I, if the state is the Init state, executing zero returning motion of the mechanical arm. At this time, the following operations are performed:
And reading a clock of the system, calculating a motion instruction of the motor at the moment according to a zero-returning motion track planning result, and sending the single motion instruction to the CANOpen master station.
and if the movement is successful in returning to zero, sending an 'e _ nominal' event to the equipment state machine, converting the equipment state machine into active.
And II, if the state is active. At this time, the following operations are performed:
and reading the control command from the input channel of the control command data, and storing the control command in a motor command buffer queue.
And reading the number of the existing instructions in the CANOpen main station instruction buffer area, and if the number of the existing instructions is less than 10, taking out 15 motion instructions from the motor instruction buffer queue at one time and sending the motion instructions to the CANOpen main station. And if the number of the instructions of the motor instruction buffer queue is less than 15, all the instructions are sent to the CANOpen main station at one time.
III, if the state is active. At this time, the communication management module is in a recovery state.
at this time, the system state is restored according to the diagnostic information, and if the restoration is successful, an "e _ nominal" event is sent to the device state machine, and the device state machine is converted into active. And reports the event to the overall control module.
if the recovery is unsuccessful, an "e _ Fault" event is sent to the state machine, the state machine is converted into a Fault state, the event is reported to the master control module, and the Updatehook () is directly exited.
And IV, if the state is active. At this time, the module is in a pause state, and the following operations are performed: and checking whether a new command exists in an input channel of the control command data, if so, reading the control command and storing the control command in a motor command buffer queue.
v, if the state is active. At this time, the module is in manual mode, and the following operations are performed:
And reading a clock of the system, calculating a motion instruction of the motor at the moment according to a motion track planning result, and sending the single motion instruction to the CANOpen master station.
VI, if the state is the Fault state, directly exiting UpdateHook ().
c) Reading the state of the motor, calculating the motion state information of the joints and the tail end of the mechanical arm according to the model of the mechanical arm, and transmitting the motion state information to the algorithm module and the master control module through an output data channel;
d) and checking whether the control master station has error report information or not, and if so, transmitting the diagnosis information to the algorithm module and the master control module. And sending an 'e _ recovery' event to the equipment state machine, converting the equipment state machine into an active.
(4) for the CleanUpHook () member function of Ec _ component, the function is automatically called when the module finishes running, and the following operations are set to be executed:
a) Turning off the motor drive enable;
b) the motor drive connection is closed.
(5) referring to fig. 8, fig. 8 is a schematic diagram of a state change of a device state machine. There are six states of Init (initialization), Fault (interruption), active.recovery (resume), active.handles (manual teaching), active.halt (pause), and active.nominal (operation). Active, recovery, Active, halt, Active, nominal four states constitute an Active (enable) state set, the Active state transition rule is all effective to four sub-states.
(6) writing a starting file of the module by using the Lua language, wherein the starting file is set to execute the following actions:
a) Loading a communication management module by using an import method of an OROCOS;
b) defining the refreshing frequency of a communication management module and the priority level of a thread;
c) Assigning the attribute of the communication management module;
d) The input and output interfaces of the communication management module are connected with the interfaces of the master control module, the algorithm module and the like by the connect method of the OROCOS.
e) and operating a communication management module by using a start method of the OROCOS, wherein the communication management module calls a Starthook () function at first and then periodically calls an Updatehook () function in real time according to a defined refresh frequency.
for the general control module, the algorithm module and the communication management module, after the operation, if a user needs to stop the module midway, the ctrl key and the D key of the keyboard are pressed simultaneously.
In conclusion, based on ROS and OROCOS, the real-time performance of the software program is ensured by utilizing the real-time characteristic of OROCOS; the openness of the ROS is fully utilized, and a master control module, an algorithm module and a communication management module developed based on the ROS and the OROCOS are communicated in real time to form complete robot controller software together; and a controller state machine and an equipment state machine are further established, so that the effective management of the business logic of the master control module and the communication management module is realized.
RTT by OROCOS: : input, RTT: : the Output method establishes data input and Output channels of the master control module, the algorithm module and the communication management module, defines a function calling interface through an Operational Call method of the OROCOS, and defines the attributes of the master control module, the algorithm module and the communication management module through Properties methods of the OROCOS. Therefore, independence and decoupling among the master control module, the algorithm module and the communication management module are guaranteed.
the technical features of the embodiments described above may be arbitrarily combined, and for the sake of brevity, all possible combinations of the technical features in the embodiments described above are not described, but should be considered as being within the scope of the present specification as long as there is no contradiction between the combinations of the technical features.
The above-mentioned embodiments only express several embodiments of the present invention, and the description thereof is more specific and detailed, but not construed as limiting the scope of the invention. It should be noted that, for a person skilled in the art, several variations and modifications can be made without departing from the inventive concept, which falls within the scope of the present invention. Therefore, the protection scope of the present patent shall be subject to the appended claims.
Claims (12)
1. a robot pause motion control method is characterized by comprising the following steps:
receiving a pause motion instruction transmitted by a control end;
executing the pause motion instruction when the current robot is in any one current motion state of point-to-point motion, linear motion, circular motion or motion returning to the original point;
acquiring a target point of current movement and the movement residual time, and storing the target point and the movement residual time as track calculation parameters for restoring the current movement; reading the current position, the current speed and the current acceleration of the tail end of the mechanical arm, and calculating a pause motion track which is passed by the tail end of the mechanical arm when the tail end of the mechanical arm is changed from the current position, the current speed and the current acceleration to stop;
and calculating the target angle, the target angular velocity and the target angular acceleration of the mechanical arm joint at each position when the tail end of the mechanical arm runs on the pause motion track, and forwarding the target angle, the target angular velocity and the target angular acceleration to the control master station.
2. the method of claim 1, wherein if the current motion is a point-to-point motion or a return-to-origin motion,
The method for acquiring and recording the target point of the current movement and the remaining time of the movement, reading the current position, the current speed and the current acceleration of the tail end of the mechanical arm, and calculating the pause movement locus according to the current position, the current speed and the current acceleration comprises the following steps:
calculating a target point moving from a current point to a point or returning to an original point, calculating the remaining time of the current movement according to the total time length of the current movement and the elapsed time of the current movement, and recording the target point and the remaining time;
reading the current joint angle, joint angular velocity and joint angular acceleration of the mechanical arm;
Calculating the time for reducing the speed of each joint to be 0 according to the principle that the maximum acceleration reduces the speed to be 0, and recording the time as the execution time of the pause operation instruction;
Obtaining a pause running track according to the pause running instruction execution time;
the method for calculating the target angle, the target angular velocity and the target angular acceleration of each joint of the mechanical arm comprises the following steps:
calculating the target angular acceleration of each mechanical arm joint according to the current joint angular velocity of the mechanical arm and the execution time of the pause operation instruction;
And calculating the target angular velocity and the joint target angle of each mechanical arm joint according to the target angular acceleration.
3. the robot pause motion control method of claim 2 wherein the current point-to-point or point-to-origin-moved target point θ1the remaining time of the current motion: tr ═ T0-t0wherein T is0Is the total duration of the current movement, t0Is the time the current motion has elapsed;
the time calculation formula for the velocity drop of each joint to be 0 is as follows:
wherein i represents the joint number, T is selectedithe maximum value of the operation instruction is recorded as a pause operation instruction execution time T;
target angular acceleration of mechanical arm jointthe calculation formula is as follows:
target angular velocity of each mechanical arm jointtarget angle theta with jointithe calculation formulas of (A) and (B) are respectively as follows:
In the formula, theta0Is the current joint angle of the mechanical arm,in order to determine the angular velocity of the joint,is the angular acceleration of the joint; t denotes the moment during the pause movement.
4. A method for controlling pause motion of a robot according to claim 1, characterized in that if the current motion is a linear motion;
The method for acquiring and recording the target point of the current movement and the remaining time of the movement, reading the current position, the current speed and the current acceleration of the tail end of the mechanical arm, and calculating the pause movement locus according to the current position, the current speed and the current acceleration comprises the following steps:
calculating a target point of the current linear motion, calculating the remaining time of the current motion according to the total time length of the current motion and the elapsed time of the current motion, and recording the target point and the remaining time;
Calculating the track, the joint angular velocity and the joint angular acceleration of the mechanical arm at the current moment according to the linear motion track;
Calculating a track which needs to be walked by the suspended linear motion according to the track, the joint angular velocity and the joint angular acceleration to obtain a suspended motion track, and recording the track which is walked by the mechanical arm along the straight line when the motion is recovered;
the method for calculating the target angle, the target angular velocity and the target angular acceleration of each joint of the mechanical arm comprises the following steps:
calculating the track position, the track speed and the track acceleration of the mechanical arm at any moment t in the pause motion:
Calculating the target position, speed and acceleration of the tail end of the mechanical arm according to the track position, the track speed and the track acceleration;
and calculating a target angle, a target angular velocity and a target angular acceleration of the mechanical arm joint according to the target position, the velocity and the acceleration by using an inverse kinematics method.
5. the robot pause motion control method according to claim 4, characterized in that the target point s of the current linear motioneThe remaining time of the current motion: tr ═ T0-t0wherein, T0Is the total duration of the current movement, t0is the time the current motion has elapsed;
the calculation formula of the track which needs to be walked when the linear motion is suspended is as follows:
Wherein, amaxThe maximum acceleration of the linear motion; record s2＝s0+s1Wherein s is2the track that the mechanical arm has already walked along the straight line when recovering the movement;
trajectory position, trajectory speed and trajectory acceleration at any time t:
wherein s is0is the track of the mechanical arm at the current moment,In order to determine the angular velocity of the joint,Is the angular acceleration of the joint, s is the track position,In order to be the speed of the track,is the track acceleration;
the calculation formula of the target position, the speed and the acceleration of the tail end of the mechanical arm is as follows:
wherein, Ptis the position of the target, and is,in order to be the speed of the vehicle,In order to be able to accelerate the vehicle,Is the unit vector of the current linear motion;
The calculation formula of the target angle, the target angular velocity and the target angular acceleration of the mechanical arm joint is as follows:
θt＝invKinematics(Pt)
wherein, thetatin order to obtain the target angle,in order to obtain the target angular velocity,Is a target ofangular acceleration, invKinematics () represents a calculation formula of inverse kinematics, J is a jacobian matrix,Is a first derivative of the Jacobian matrix, J+Is the pseudo-inverse of.
6. the method of claim 1, wherein if the current motion is a circular motion;
The method for acquiring and recording the target point of the current movement and the remaining time of the movement, reading the current position, the current speed and the current acceleration of the tail end of the mechanical arm, and calculating the pause movement locus according to the current position, the current speed and the current acceleration comprises the following steps:
Calculating a target point of the current circular motion, calculating the remaining time of the current motion according to the total duration of the current motion and the elapsed time of the current motion, and recording the target point and the remaining time;
Calculating the current track, joint angular velocity and joint angular acceleration of the mechanical arm according to the circular arc motion track;
Calculating a track which needs to be walked by the suspended circular motion according to the track, the joint angular velocity and the joint angular acceleration to obtain a suspended running track, and recording the track which is walked by the mechanical arm along the circular arc when the motion is recovered;
the method for calculating the target angle, the target angular velocity and the target angular acceleration of each joint of the mechanical arm comprises the following steps:
calculating the track position, the track speed and the track acceleration of the mechanical arm at any moment t in the pause motion;
Calculating the target position, speed and acceleration of the tail end of the mechanical arm according to the track position, the track speed and the track acceleration;
And calculating a target angle, a target angular velocity and a target angular acceleration of the mechanical arm joint according to the target position, the velocity and the acceleration by using an inverse kinematics method.
7. the robot pause motion control method according to claim 6, characterized in that the target point s of the current circular motionethe remaining time of the current motion: tr ═ T0-t0wherein, T0Is the total duration of the current movement, t0is the time the current motion has elapsed;
the calculation formula of the track which needs to be walked when the circular motion is paused is as follows:
wherein, amaxthe maximum acceleration of the circular motion; record s2＝s0+s1Wherein s is2The track that the mechanical arm has passed along the arc is recovered when the motion is performed;
Trajectory position, trajectory speed and trajectory acceleration at any time t:
wherein s is0is the track of the mechanical arm at the current moment,in order to determine the angular velocity of the joint,Is the angular acceleration of the joint, s is the track position,in order to be the speed of the track,is the track acceleration;
target position P of end of robot armt＝[xt,yt,zt]T：
Wherein, thetatis a circular arc P0PtCorresponding central angle of the circlea1,a2,a3,b1,b2,b3the parameters such as R are all known parameters of the current circular arc motion track;
Target velocity of end of mechanical armAnd accelerationThe calculation formula is as follows:
Wherein the content of the first and second substances,Is a point PtA tangent vector of;
The calculation formula of the target angle, the target angular velocity and the target angular acceleration of the mechanical arm joint is as follows:
θt＝invKinematics(Pt)
wherein, thetatIn order to obtain the target angle,In order to obtain the target angular velocity,for the target angular acceleration, invKinematics () represents a calculation formula of inverse kinematics, J is a jacobian matrix,is a first derivative of the Jacobian matrix, J+Is the pseudo-inverse of.
8. The robot pause motion control method according to any one of claims 1 to 7, characterized by further comprising:
after the pause motion instruction is executed, asynchronously triggering a pause motion execution function and calling a pause motion planning function through a first interface; wherein the first interface is a real-time input/output interface based on creating OROCOS on ROS;
Executing the pause motion planning function and calculating a pause motion track;
sending the target angle, the target angular velocity and the target angular acceleration of the mechanical arm joint to equipment communication software through a second interface and then forwarding the equipment communication software to a control master station; wherein the second interface is a real-time input/output interface based on creating OROCOS on ROS.
9. The robot pause motion control method according to claim 8, wherein the method of calculating the target angle, the target angular velocity, and the target angular acceleration of the robot arm joint comprises:
counting after the mechanical arm starts to move on the pause motion tracktime t of movement of the robot armn；
If the movement time satisfies: t is tnCalculating the target angle, the target angular velocity and the target angular acceleration of each joint of the mechanical arm at intervals of set time tau; if the movement time tnsatisfies the following conditions: t is tnIf the distance is more than T, stopping moving;
wherein T is the pause instruction execution time.
10. A robot pause motion control system, comprising: the system comprises a master control module, an algorithm module and a communication management module;
The master control module is used for receiving a motion pause instruction transmitted by the control end; executing the pause motion instruction when the current robot is in any one current motion state of point-to-point motion, linear motion, circular motion or motion returning to the original point;
The algorithm module is used for acquiring a target point of current motion and the residual time of the motion, and storing the target point and the residual time of the motion as track calculation parameters for restoring the current motion; reading the current position, the current speed and the current acceleration of the tail end of the mechanical arm, and calculating a pause motion track which is passed by the tail end of the mechanical arm when the tail end of the mechanical arm is changed from the current position, the current speed and the current acceleration to stop; calculating the target angle, the target angular velocity and the target angular acceleration of the mechanical arm joint at each position when the tail end of the mechanical arm runs on the pause motion track;
and the communication management module is used for forwarding the target angle, the target angular velocity and the target angular acceleration to the control master station.
11. a computer-readable storage medium, on which a computer program is stored which, when being executed by a processor, carries out the steps of the method according to any one of claims 1 to 9.
12. a computer device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, characterized in that the steps of the method according to any of claims 1-9 are implemented when the program is executed by the processor.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710256933.0A CN106945044B (en) | 2017-04-19 | 2017-04-19 | Robot pause motion control method and system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710256933.0A CN106945044B (en) | 2017-04-19 | 2017-04-19 | Robot pause motion control method and system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN106945044A CN106945044A (en) | 2017-07-14 |
| CN106945044B true CN106945044B (en) | 2019-12-17 |
Family
ID=59477107
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710256933.0A Active CN106945044B (en) | 2017-04-19 | 2017-04-19 | Robot pause motion control method and system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN106945044B (en) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109822230B (en) * | 2019-01-31 | 2021-12-14 | 大族激光科技产业集团股份有限公司 | Motion control method and device of laser cutting nozzle and computer equipment |
| CN110861083B (en) * | 2019-10-25 | 2020-11-24 | 广东省智能制造研究所 | Robot teaching method and device, storage medium and robot |
| CN111015658B (en) * | 2019-12-20 | 2021-06-29 | 深圳市优必选科技股份有限公司 | Method and device for controlling robot |
| CN112497210B (en) * | 2020-11-09 | 2022-04-26 | 北京配天技术有限公司 | Robot movement method, robot and device with storage function |
| CN112799325B (en) * | 2020-12-31 | 2022-09-06 | 泉芯集成电路制造(济南)有限公司 | Method and device for monitoring motion of mechanical arm and related equipment |
| CN114102597B (en) * | 2021-11-30 | 2023-05-16 | 伯朗特机器人股份有限公司 | Mechanical arm articulation operation control method, electronic equipment and storage medium |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5887122A (en) * | 1994-12-14 | 1999-03-23 | Fanuc Ltd. | Tracking control method for robot with weaving action |
| CN104526695A (en) * | 2014-12-01 | 2015-04-22 | 北京邮电大学 | Space manipulator track planning method for minimizing base seat collision disturbance |
| CN105182887A (en) * | 2015-08-05 | 2015-12-23 | 上海腾滨自动化技术有限公司 | Automatic control method for industrial robot |
| CN105459113A (en) * | 2014-09-25 | 2016-04-06 | 发那科株式会社 | Robot controller |
| CN106068175A (en) * | 2014-03-14 | 2016-11-02 | 索尼公司 | Robot arm equipment, robot arm control method and program |
-
2017
- 2017-04-19 CN CN201710256933.0A patent/CN106945044B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5887122A (en) * | 1994-12-14 | 1999-03-23 | Fanuc Ltd. | Tracking control method for robot with weaving action |
| CN106068175A (en) * | 2014-03-14 | 2016-11-02 | 索尼公司 | Robot arm equipment, robot arm control method and program |
| CN105459113A (en) * | 2014-09-25 | 2016-04-06 | 发那科株式会社 | Robot controller |
| CN104526695A (en) * | 2014-12-01 | 2015-04-22 | 北京邮电大学 | Space manipulator track planning method for minimizing base seat collision disturbance |
| CN105182887A (en) * | 2015-08-05 | 2015-12-23 | 上海腾滨自动化技术有限公司 | Automatic control method for industrial robot |
Also Published As
| Publication number | Publication date |
|---|---|
| CN106945044A (en) | 2017-07-14 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106945044B (en) | Robot pause motion control method and system | |
| WO2018192178A1 (en) | Point-to-point motion control method and system for robot | |
| CN106926243B (en) | Robot teaching motion control method and system | |
| CN106914900B (en) | Robot control system based on ROS and OROCOS | |
| CN106950924B (en) | Robot circular motion control method and system | |
| CN106959694B (en) | Robot linear motion control method and system | |
| CN106945045B (en) | communication method and system for robot control based on ROS and OROCOS | |
| CN109471435B (en) | Multi-heterogeneous mobile robot control system | |
| JP5149258B2 (en) | Robot component management device | |
| CN111427310A (en) | Industrial robot controller operation system | |
| US20200189100A1 (en) | Technique for Providing Reliable Control in a Cloud Robotics System | |
| WO2019205669A1 (en) | Method, device, system and electronic device for controlling elevator | |
| US20170113350A1 (en) | System and method of controlling robot | |
| CN107030695B (en) | Robot returns to origin motion control method and system | |
| US20130073091A1 (en) | Robot control apparatus and robot system | |
| CN113642243A (en) | Multi-robot deep reinforcement learning system, training method, device and medium | |
| CN109454641B (en) | Multi-task division and data interaction method for motion controller | |
| CN106965181B (en) | Robot control method and system based on ROS and OROCOS | |
| CN106956265B (en) | The execution method and system of robot motion planning based on ROS and OROCOS | |
| Velamala et al. | Development of ROS-based GUI for control of an autonomous surface vehicle | |
| CN117120219A (en) | Real-time robot control frame | |
| CN110913146B (en) | Fixed-point linkage shooting method for rail car and holder | |
| US9191237B1 (en) | Wireless communication systems and methods | |
| KR100869587B1 (en) | Robot Middleware Framework System | |
| Trojanek et al. | Design of asynchronously stimulated robot behaviours |
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 |