CN106945044A - Robot pause motion control method and system - Google Patents

Robot pause motion control method and system Download PDF

Info

Publication number
CN106945044A
CN106945044A CN201710256933.0A CN201710256933A CN106945044A CN 106945044 A CN106945044 A CN 106945044A CN 201710256933 A CN201710256933 A CN 201710256933A CN 106945044 A CN106945044 A CN 106945044A
Authority
CN
China
Prior art keywords
motion
target
current
acceleration
joint
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.)
Granted
Application number
CN201710256933.0A
Other languages
Chinese (zh)
Other versions
CN106945044B (en
Inventor
阳方平
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Guangzhou Shiyuan Electronics Thecnology Co Ltd
Original Assignee
Guangzhou Shiyuan Electronics Thecnology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Guangzhou Shiyuan Electronics Thecnology Co Ltd filed Critical Guangzhou Shiyuan Electronics Thecnology Co Ltd
Priority to CN201710256933.0A priority Critical patent/CN106945044B/en
Publication of CN106945044A publication Critical patent/CN106945044A/en
Application granted granted Critical
Publication of CN106945044B publication Critical patent/CN106945044B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • BPERFORMING OPERATIONS; TRANSPORTING
    • B25HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25JMANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00Programme-controlled manipulators
    • B25J9/16Programme controls
    • B25J9/1602Programme controls characterised by the control system, structure, architecture
    • BPERFORMING OPERATIONS; TRANSPORTING
    • B25HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25JMANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00Controls for manipulators
    • B25J13/08Controls for manipulators by means of sensing devices, e.g. viewing or touching devices

Abstract

The present invention relates to a kind of robot pause motion control method and system, methods described includes:Receive the pause motion instruction of control end transmission;Point-to-point motion, linear motion, circular motion are in current robot or are returned under any one current motion state of origin motion;The target point of current kinetic and the remaining time of motion are obtained, the remaining time of target point and motion is saved as to the trajectory calculation parameter for recovering current kinetic;Read current location, present speed and the current acceleration of mechanical arm tail end, and the pause motion track that calculating machine arm end is passed through when changing to stopping by current location, present speed and current acceleration;When calculating machine arm end is run on pause motion track, angle on target, target angular velocity and target angular acceleration of the joint of mechanical arm in each position are forwarded to control main website;The present invention may be constructed a complete robot pause motion control system, reduce robot control system development cost, improve control effect.

Description

Robot pause motion control method and system
Technical field
The present invention relates to technical field of robot control, more particularly to a kind of robot pause motion control method and it is System.
Background technology
Robot Operating System (ROS) are the robot operating systems increased income, and can be robot development person One programming framework standardizing, increasing income is provided.But ROS does not support that real-time thread is operated at present.Open Robot Control Software (OROCOS) are also a kind of robot control software's programming framework increased income, and its feature is to support real When threading operation, but its opening, versatility do not have ROS good.
Therefore, robot pause motion is a kind of important motion mode of robot, at present in some application schemes, OROCOS is used on ROS, but prior art is on the framework built, it is impossible to constitute a complete robot controller soft Part, when performing pause motion, it is impossible to make full use of ROS and OROCOS characteristic, system development costs are high, and control effect is poor.
The content of the invention
Based on this, it is necessary to for above-mentioned technical problem there is provided a kind of robot pause motion control method, system is reduced Development cost, improves control effect.
A kind of robot pause motion control method, including:
Receive the pause motion instruction of control end transmission;
Current robot be in point-to-point move, linear motion, circular motion or return to origin motion any one Under current motion state, the pause motion instruction is performed;
The target point of current kinetic and the remaining time of motion are obtained, the remaining time of the target point and motion is preserved To recover the trajectory calculation parameter of current kinetic;Current location, present speed and the current acceleration of mechanical arm tail end are read, and The pause motion rail that calculating machine arm end is passed through when changing to stopping by current location, present speed and current acceleration Mark;
When calculating machine arm end is run on the pause motion track, target angle of the joint of mechanical arm in each position Degree, target angular velocity and target angular acceleration are forwarded to control main website.
A kind of robot pause motion control system, including:Top control module, algoritic module and communication management module;
The top control module, the pause motion instruction for receiving control end transmission;Point-to-point is in current robot Motion, linear motion, circular motion are returned under any one current motion state of origin motion, perform the pause motion Instruction;
The algoritic module, for obtaining the target point of current kinetic and the remaining time of motion, by the target point and The remaining time of motion saves as the trajectory calculation parameter for recovering current kinetic;Read the current location, current of mechanical arm tail end Speed and current acceleration, and calculating machine arm end by current location, present speed and current acceleration when changing to stopping The pause motion track passed through;When calculating machine arm end is run on the pause motion track, joint of mechanical arm is each Angle on target, target angular velocity and the target angular acceleration of individual position;
The communication management module, for the angle on target, target angular velocity and target angular acceleration to be forwarded into control Main website processed.
Above-mentioned robot pause motion control method and system, after the pause motion instruction of control end transmission is received, Point-to-point motion, linear motion, circular motion or any one current kinetic for returning to origin motion are in current robot Under state, the movement locus of pause motion, and the mesh that each joint of real-time computer tool arm is run on the movement locus are calculated The state parameters such as angle, target angular velocity and target angular acceleration are marked, and are forwarded to control main website;Realize the pause to robot Motion control process;The technical scheme may be constructed a complete robot pause motion control system, the control of reduction robot System development costs processed, improve control effect.
Brief description of the drawings
Fig. 1 is the robot pause motion control method flow chart of the embodiment of the present invention;
Fig. 2 is robot pause motion control system architecture schematic diagram;
Fig. 3 is that top control module performs algorithm flow chart;
Fig. 4 is that algoritic module performs algorithm flow chart;
Fig. 5 is the hardware structure model of the robot control system of an application example;
Fig. 6 is the architecture diagram built based on ROS and OROCOS;
Fig. 7 is the state change schematic diagram of controller state machine;
Fig. 8 is the state change schematic diagram of equipment state machine.
Embodiment
The embodiment of the robot pause motion control method of the present invention is illustrated below in conjunction with the accompanying drawings.
In the embodiment of the present invention, the pause motion refers to mechanical arm pause current kinetic instruction, and speed is reduced to 0, works as receipts To recovering after instruction, it will continue to move along the track when preplanning, the target location until reaching present instruction.
With reference to shown in Fig. 1, Fig. 1 is the robot pause motion control method flow chart of the embodiment of the present invention, including:
S10, receives the pause motion instruction of control end transmission;
In above-mentioned steps, it is possible to use default communication protocol simultaneously receives pause fortune in the way of asynchronous remote procedure call Dynamic instruction;
In the process, can be that operator generates pause motion instruction by the human-computer interaction interface of control end, this refers to Order is without Transfer Parameters;By default communication protocol, IEC (The Internet Communications are such as based on Engine, Internet communication engine) exploitation communication protocol, connect in the way of asynchronous remote procedure call from human-computer interaction interface Receive pause motion instruction.
S20, is in point-to-point motion, linear motion, circular motion in current robot or returns to any of origin motion Under a kind of current motion state, the pause motion instruction is performed;
In this step, by judge current robot motion state whether be executable pause motion point-to-point move, Linear motion, circular motion return to origin motion state, if so, performing pause motion instruction;
In one embodiment, pause motion shape can be performed when being converted to the state of real-time inspection and control device state machine During state, call pause motion to plan function, start to perform jerk motion planning flow;
For judge current robot motion state whether be executable pause motion designated movement state, can pass through Judge controller state machine whether be point-to-point motion, linear motion, circular motion, return to origin motion four kinds of states it One, if so, performing the step of calling the pause motion planning function, otherwise, refusal performs operation suspension instruction.
S30, obtains the target point of current kinetic and the remaining time of motion, by the target point and the remaining time of motion Save as the trajectory calculation parameter for recovering current kinetic;Read the current location of mechanical arm tail end, present speed and work as preacceleration Degree, and the pause fortune that calculating machine arm end is passed through when changing to stopping by current location, present speed and current acceleration Dynamic rail mark;
In one embodiment, perform after the pause motion instruction, function can be performed simultaneously with asynchronous triggering pause motion Pause motion is called to plan function by first interface;Wherein, the first interface be based on ROS create OROCOS it is real-time Input/output interface;
Further, letter can be planned by pause motion described in OROCOS Operational Caller method calls Number;
The pause motion planning function is performed, obtains and records the target point of current kinetic and the remaining time of motion, And current location, present speed and the current acceleration of mechanical arm tail end are read, and according to the current location, present speed Pause motion track is calculated with current acceleration;
In one embodiment, the pause motion planning function is performed, the controller state machine is converted into execution Pause motion (Active.Halt) state.
As embodiment, if the current kinetic, which is point-to-point, moves or return to origin motion, pause motion rail is calculated The method of mark may include steps of:
(A1) calculate current point-to-point or return to the target point of origin motion, according to the total duration of current kinetic and current fortune The remaining time of the dynamic Time Calculation current kinetic undergone, record the target point and remaining time;
The current point-to-point or the target point θ for returning to origin motion1, the remaining time of current kinetic:Tr=T0-t0, its Middle T0It is the total duration of current kinetic, t0It is the time that current kinetic has undergone;
(A2) the current joint angles of mechanical arm, joint angular speed, and joint angular acceleration are read;
Specifically, reading the current joint angles θ of mechanical arm0, joint angular speedJoint angular acceleration
(A3) current time is designated as 0, speed is reduced to 0 principle according to peak acceleration, calculate each joint velocity It is reduced to for 0 time, is designated as operation suspension time for each instruction;
The Time Calculation formula that each described joint velocity is reduced to 0 is:
Wherein, i represents the label in joint, chooses TiIn maximum, be designated as operation suspension time for each instruction T;
(A4) operation suspension track is obtained according to operation suspension time for each instruction T.
In one embodiment, if the current kinetic is linear motion, the method for calculating pause motion track can be with Comprise the following steps:
(B1) calculate the target point that currently moves along a straight line, according to the total duration of current kinetic and current kinetic undergone when Between calculate current kinetic remaining time, record the target point and remaining time;
Specifically, the target point s currently moved along a straight linee, the remaining time of current kinetic:Tr=T0-t0, wherein, T0 It is the total duration of current kinetic, t0It is the time that current kinetic has undergone.
(B2) according to the track at straight-line trajectory calculating machine arm current time, joint angular speed and joint angle accelerate Degree;
Specifically, current straight-line trajectory is planned by algorithm known, therefore can be with calculating machine arm current time Track s0, joint angular speedJoint angular acceleration
(B3) according to the track, the rail that joint angular speed and joint angle acceleration calculation pause linear motion needs are passed by Mark, obtains pause motion track, and record the track that mechanical arm has been passed by along straight line when recovering motion;
Specifically, the trajectory calculation formula that pause linear motion needs are passed by is:
Wherein, amaxFor the acceleration that linear motion is maximum;Record s2=s0+s1, wherein, s2To recover mechanical arm during motion The track passed by along straight line.
In one embodiment, if the current kinetic is circular motion, the method for calculating pause motion track can be with Comprise the following steps:
(C1) calculate the target point of current circular motion, according to the total duration of current kinetic and current kinetic undergone when Between calculate current kinetic remaining time, record the target point and remaining time;
The target point s of the current circular motione, the remaining time of current kinetic:Tr=T0-t0, wherein, T0It is current The total duration of motion, t0It is the time that current kinetic has undergone;
(C2) according to the track at circular motion trajectory calculation mechanical arm current time, joint angular speed and joint angle accelerate Degree;
Specifically, current circular motion track is planned by algorithm known, therefore can be with calculating machine arm current time Track s0, joint angular speedJoint angular acceleration
(C3) according to the track, the rail that joint angular speed and joint angle acceleration calculation pause circular motion needs are passed by Mark, obtains operation suspension track, and record the track that mechanical arm has been passed by along circular arc when recovering motion;
Pause circular motion needs the trajectory calculation formula passed by be:
Wherein, amaxFor the acceleration that circular motion is maximum;Record s2=s0+s1, wherein, s2To recover mechanical arm during motion The track passed by along circular arc.
S40, when calculating machine arm end is run on the pause motion track, mesh of the joint of mechanical arm in each position Mark angle, target angular velocity and target angular acceleration are forwarded to control main website.
In one embodiment, by second interface by the angle on target, target angular velocity and mesh of the joint of mechanical arm Mark angular acceleration is sent to equipment communication software forward to controlling main website;Wherein, the second interface is based on establishment on ROS OROCOS real-time input/output interface.
As embodiment, when being run for calculating machine arm end on the pause motion track, joint of mechanical arm exists The method of the angle on target of the joint of mechanical arm of each position, target angular velocity and target angular acceleration, can include following step Suddenly:
(a) after mechanical arm starts to move on the pause motion track, the run duration t of mechanical arm is countedn
If (b) run duration is met:tn≤ T, the target in each joint of one-time mechanical arm is calculated every setting time τ Angle, target angular velocity and target angular acceleration;τ=1/f, f represent refreshing frequency;Generally, the τ is 1 millisecond;
If (c) run duration tnMeet:tn> T, then stop motion;Wherein, T is operation suspension time for each instruction.
Such as above-mentioned embodiment, if the current kinetic, which is point-to-point, moves or return to origin motion, calculating machine arm is every The method of the angle on target in individual joint, target angular velocity and target angular acceleration comprises the following steps:
(A5) each machinery is calculated according to the current joint angular speed of mechanical arm and the operation suspension time for each instruction The target angular acceleration of shoulder joint;
Specifically, the target angular acceleration of joint of mechanical armComputing formula is:
(A6) target angular velocity and joint angle on target of each joint of mechanical arm are calculated according to the target angular acceleration;
Specifically, the target angular velocity of each joint of mechanical armWith joint angle on target θiComputing formula be respectively:
In formula, θ0For the current joint angles of mechanical arm,For joint angular speed,For joint angular acceleration;T represents temporary At the time of stoppage in transit during moving.
Such as above-mentioned embodiment, if the current kinetic is linear motion, the angle on target in each joint of calculating machine arm, The method of target angular velocity and target angular acceleration comprises the following steps:
(B4) track position, path velocity and the path acceleration of mechanical arm any instant t in pause motion is calculated:
Specifically, any instant t track position, path velocity and path acceleration:
Wherein, s0For the track at mechanical arm current time,For joint angular speed,For joint angular acceleration, s is track Position,For path velocity,For path acceleration;
(B5) according to the track position, the target location of path velocity and path acceleration calculating machine arm end, speed Degree and acceleration;
Specifically, the target location of mechanical arm tail end, the computing formula of velocity and acceleration are:
Wherein, PtFor target location,For speed,For acceleration,It is the unit vector currently moved along a straight line;
(B6) using inverse kinematics method according to the target location, the target of velocity and acceleration calculating machine shoulder joint Angle, target angular velocity and target angular acceleration;
Specifically, the computing formula of the angle on target of the joint of mechanical arm, target angular velocity and target angular acceleration is:
θt=invKinematics (Pt)
Wherein, θtFor angle on target,For target angular velocity,For target angular acceleration, invKinematics () is represented The computing formula of inverse kinematics, J is Jacobian matrix,Led for the single order of Jacobian matrix, J+For pseudoinverse.
Such as above-mentioned embodiment, if the current kinetic is circular motion, the angle on target in each joint of calculating machine arm, The method of target angular velocity and target angular acceleration comprises the following steps:
(C4) track position, path velocity and the path acceleration of mechanical arm any instant t in pause motion is calculated;
Specifically, any instant t track position, path velocity and path acceleration:
Wherein, s0For the track at mechanical arm current time,For joint angular speed,For joint angular acceleration, s is track Position,For path velocity,For path acceleration;
(C5) according to the track position, the target location of path velocity and path acceleration calculating machine arm end, speed Degree and acceleration;
Specifically, the target location P of mechanical arm tail endt=[xt,yt,zt]T
Wherein, θtFor circular arc P0PtCorresponding central angle, meetsa1,a2,a3,b1,b2,b3, the parameter such as R is to work as Preceding circular motion track known parameters;
The target velocity of mechanical arm tail endAnd accelerationComputing formula is:
Wherein,It is point PtTangent vector;
(C6) using inverse kinematics method according to the target location, the target of velocity and acceleration calculating machine shoulder joint Angle, target angular velocity and target angular acceleration.
The computing formula of the angle on target of the joint of mechanical arm, target angular velocity and target angular acceleration is:
θt=invKinematics (Pt)
Wherein, θtFor angle on target,For target angular velocity,For target angular acceleration, invKinematics () is represented The computing formula of inverse kinematics, J is Jacobian matrix,Led for the single order of Jacobian matrix, J+For pseudoinverse.
The technical scheme of above-described embodiment, using OROCOS real-time input/output interface, by set communication protocol with The mode of asynchronous remote procedure call receives pause motion instruction, starts pause motion instruction triggers function, calls robot to transport The pause motion of dynamic planning performs function, and the movement locus of pause motion is calculated according to the function, and real-time computer tool arm is each The state parameters such as angle on target, target angular velocity and target angular acceleration that individual joint is run on the movement locus, by setting Above-mentioned state parameter is forwarded to control main website by standby communication software, realizes the pause motion control process to robot;The technology Scheme can be controlled the reception of instruction, parsing, and algorithm calls, performs, the function such as parameter is calculated and transmitted in real time, can be with structure Into a complete robot pause motion control system, robot control system development cost is reduced, control effect is improved;Separately The outer application for combining controller state machine, realizes the optimal control to algorithm process process, further increases control effect.
Robot pause motion control method is directed to, the invention provides the corresponding robot pause fortune of the control method Autocontrol system,
With reference to shown in Fig. 2, Fig. 2 is robot pause motion control system architecture schematic diagram, including:Top control module, algorithm Module and communication management module;
The top control module, the pause motion instruction for receiving control end transmission;Point-to-point is in current robot Motion, linear motion, circular motion are returned under any one current motion state of origin motion, perform the pause motion Instruction;
The algoritic module, for obtaining the target point of current kinetic and the remaining time of motion, by the target point and The remaining time of motion saves as the trajectory calculation parameter for recovering current kinetic;Read the current location, current of mechanical arm tail end Speed and current acceleration, and calculating machine arm end by current location, present speed and current acceleration when changing to stopping The pause motion track passed through;When calculating machine arm end is run on the pause motion track, joint of mechanical arm is each Angle on target, target angular velocity and the target angular acceleration of individual position;
The communication management module, for the angle on target, target angular velocity and target angular acceleration to be forwarded into control Main website processed.
In one embodiment, the top control module is communicatively coupled by first interface with algoritic module, algorithm mould Block is communicatively coupled by second interface with communication management module, and the first interface, second interface are based on establishment on ROS OROCOS real-time input/output interface;
The top control module is performed after pause motion instruction, and asynchronous triggering pause motion performs function and passes through first interface Pause motion is called to plan function;
The algoritic module performs the pause motion planning function, and calculates pause motion track;
And sent out the angle on target, target angular velocity and target angular acceleration of the joint of mechanical arm by second interface Give communication management module.
The technical scheme of above-described embodiment, using OROCOS real-time input/output interface, top control module is logical by setting Letter agreement receives pause motion instruction in the way of asynchronous remote procedure call, starts pause motion instruction triggers function, calls The pause motion of the robot motion planning of algoritic module performs function, and the movement locus of pause motion is calculated according to the function, And angle on target, target angular velocity and the target angular acceleration that each joint of real-time computer tool arm is run on the movement locus Deng state parameter, above-mentioned state parameter is forwarded to control main website by communication management module, realizes the pause motion control to robot Process processed;The technical scheme can be controlled the reception of instruction, parsing, and algorithm is called, performed, and parameter is calculated and transmitted in real time Etc. function, a complete robot pause motion control system is may be constructed, robot control system development cost is reduced, carries High control effect.
As embodiment, top control module performs algorithm flow, may be referred to shown in Fig. 3, and Fig. 3 is that top control module performs algorithm Flow chart;It is specific as follows:
1) operator generates pause motion by human-computer interaction interface and instructed.
2) top control module is received from human-computer interaction interface in the way of asynchronous remote procedure call and suspended by communication protocol Movement instruction.
3) instruction is reached after top control module, and asynchronous triggering pause motion performs function, and the function first determines whether controller shape Whether state machine is point-to-point motion, and linear motion, circular motion returns to one of four kinds of states of origin.If it is not, then refusal is held Row this operation suspension instruction.
If 4) controller state machine be point-to-point motion, linear motion, circular motion, return to origin four kinds of states it One, then perform following operation:
A) function is planned by the pause motion of OROCOS Operational Caller method call algoritic modules.
B) controller state machine is converted into execution pause motion (Active.Halt) state.
As embodiment, algoritic module performs algorithm flow, may be referred to shown in Fig. 4, and Fig. 4 is that algoritic module performs algorithm Flow chart;It is specific as follows:
1) pause motion planning function is called by top control module, starts to perform pause motion planning process.
2) if current kinetic is back to zero motion or point-to-point motion, point-to-point is called to suspend planning algorithm:
A) the target point θ of current kinetic is recorded1
B) remaining time of current kinetic is recorded:Tr=T-t.Wherein T is the total duration of current kinetic, and t is current kinetic The time undergone;
C) the current joint angles θ of mechanical arm is read0, joint angular speedJoint angular acceleration
D) current time is designated as 0, speed is reduced to 0 principle according to peak acceleration, calculate each joint velocity drop For 0 time:
Wherein, i represents the label in joint.Choose TiIn maximum, be designated as pause instruction perform the time, be designated as T.
E) acceleration for calculating each joint is:
F) the corresponding speed of t is respectively with joint Angle Position:
If 3) current kinetic is linear motion, the algorithm for calling straight line to suspend:
A) the target point s of current kinetic is recordede
B) remaining time of current kinetic is recorded:Tr=T-t.Wherein T is the total duration of current kinetic, and t is current kinetic The time undergone;
C), therefore can be current with calculating machine arm because current straight-line trajectory can be by quintic algebra curve algorithmic rule The track s at moment0, joint angular speedJoint angular acceleration
D) track that pause linear motion needs to pass by is calculated:
Wherein, amaxJust move along a straight line maximum acceleration.
E) when record recovers motion, the track that mechanical arm has been passed by along straight line:s2=s0+s1
F) track of mechanical arm any instant t in pause motion, path velocity, path acceleration are calculated:
G) target location of calculating machine arm end, velocity and acceleration:
Wherein,It is the unit vector of linear motion.
H) inverse kinematics method is used, the position of joint angle is calculated:
θt=invKinematics (Pt) (8)
I) angular speed and angular acceleration in joint, can be obtained by following expression:
Wherein, J is Jacobi (Jacobian) matrix,Led for the single order of Jacobian matrix, J+For pseudoinverse (Pseudoinverse)。
If 4) current kinetic is circular motion, the algorithm for calling circular arc to suspend:
A) the target trajectory s of current kinetic is recordede
B) remaining time of current kinetic is recorded:Tr=T-t.Wherein T is the total duration of current kinetic, and t is current kinetic The time undergone;
C), therefore can be current with calculating machine arm because current circular motion track can be by quintic algebra curve algorithmic rule The track s at moment0, joint angular speedJoint angular acceleration
D) track that pause circular motion needs to pass by is calculated:
Wherein, amaxJust move along a straight line maximum acceleration.
E) when record recovers motion, the track that mechanical arm has been passed by along circular arc:s2=s0+s1
F) track of mechanical arm any instant t in pause motion, path velocity, path acceleration are calculated:
G) the target location P of end is calculatedt=[xt,yt,zt]T
Wherein, θtFor circular arc P0PtCorresponding central angle, meetsa1,a2,a3,b1,b2,b3, the parameter such as R can be by Circular motion quintic algebra curve planning algorithm is tried to achieve;
H) target velocity of end is tried to achieveAnd acceleration
Wherein,It is point PtTangent vector;
I) inverse kinematics method is used, the position of joint angle is calculated:
θt=invKinematics (Pt) (16)
Wherein, invKinematics () represents the computing formula of inverse kinematics.
J) angular speed and angular acceleration in joint, can be obtained by following expression:
Wherein, J is Jacobi (Jacobian) matrix,Led for the single order of Jacobian matrix, J+For pseudoinverse (Pseudoinverse)。
5) run duration tnNo more than motion duration T:
A) angle on target in each joint of one-time mechanical arm is calculated respectively every 1 millisecond, target angular velocity and target angle add Speed;
B) by the angle on target in each joint of mechanical arm, target angular velocity and target angular acceleration are sent to telecommunication management Module.
6) run duration tnMore than motion duration T, then stop motion.
As embodiment, communication management module can be further used for performing following algorithm:
The status information of read machine people's motor, believes according to the state of robot motion model's calculating robot's mechanical arm Breath, feeds back to top control module by the 3rd interface by the status information of robot, the status information of robot is connect by second Mouth feeds back to algoritic module;Wherein, the status information of the motor includes position, speed and torque etc.;The shape of the mechanical arm State information includes joint angles, joint angular speed, joint angular acceleration, end pose, end linear velocity, end angular speed, end End line acceleration and end linear acceleration etc..
In order to become apparent from the technical scheme of embodiments of the invention, the hardware and software ring using the present invention is described below Border application example:
With reference to shown in Fig. 5, Fig. 5 is the hardware structure model of the robot control system of an application example, in robot control Device processed builds software architecture, runs on (SuSE) Linux OS, and the Linux main frames can be the PC of X86-based, or ARM The development board of chip embedded framework, the control instruction of the human-computer interaction interface at top control module Access Control end.
Linux main frames can install following software:Xenomai or RTAI or RT Preempt real-time kernel is installed Patch;The softwares such as installation ROS, OROCOS, rFSM.
With reference to shown in Fig. 6, Fig. 6 is the architecture diagram built based on ROS and OROCOS;In control process, operation Top control module, algoritic module and communication management module are run in system.
1st, for top control module:
(1) top control module creates ROS Package using ROS orocreate-catkin-pkg methods, is designated as Ec_ Control_system, then in Package, by the RTT for inheriting OROCOS::TaskContext classes, are designated as Ec_ control_system_component。
In the constructed fuction of Ec_control_system_component classes, following operation is arranged to carry out:
A) OROCOS RTT is utilized::Input and RTT::Output methods, the input to module, output interface is determined Justice.
The interface wherein inputted includes:
1. the incoming diagnostic data of communication management module;
2. the incoming state feedback information of communication management module:Including motor operating state etc.;
3. the state of controller state machine;
The interface of output includes:
1. controller state machine event is triggered, and is exported and is given controller state machine.
B) function call interface is set using OROCOS Operational Caller methods.
First, the call back function for setting event to report:Event report processing request is responded, including generation error Timestamp, the information such as event level, and by event information, be sent to human-computer interaction interface and show.
Second, the call back function for setting alarm to set:According to diagnostic message, judge whether to generate alarm.Such as, position, Whether speed, acceleration transfinites etc..
3rd, the control instruction triggering function of various motion plannings is set, and these functions are by the corresponding sound of algoritic module Function is answered to be called.
C) call OROCOS Properties methods to define the attribute of top control module, top control module is defined into a machinery The attribute of shoulder joint number.
(2) in Ec_control_system_component StartHook () member function, set execution as follows Operation:
A) whether audit log report is normal, if abnormal directly exit, and relevant information is passed by event reporting interface Pass top control module processing;
B) communication connection with human-computer interaction interface is set up by the ICE communication protocols developed, calling communication agreement is provided Dynamic asynchronous remote procedure call (RPC) method, the readjustment letter that is responded of control instruction initiated human-computer interaction interface Number is bound.The call back function, according to ICE, (The Internet Communications Engine interconnect Netcom first Believe engine) incoming first parameter of remote process asynchronous invoking method that provides, judge call type, then foundation this type The movement instruction triggering function of Selection and call corresponding sports planning.
(3) for Ec_control_system_component CleanUpHook () member function, in order that must be somebody's turn to do Function is when top control module terminates operation, and realization is called automatically, may be arranged as performing following operation:
A) communication protocol interface for calling ICE to develop, closes the communication connection with human-computer interaction interface.
(4) for controller state machine, with reference to shown in Fig. 7, Fig. 7 is the state change schematic diagram of controller state machine;Can With set Init, Ready, Fault, Active.Recovery, Active.Halt, Active.Hands, Active.ToZero, Active.PTP, Active.Line, Active.Circle, Active.Stop totally ten one states, point It Dai Biao not initialize, wait instruction input, recovery, pause, manual teaching, return to origin, pause motion, pause motion, pause Motion, jerk state.Wherein, Active.Recovery, Active.Halt, Active.Hands, Active.ToZero, This eight states of Active.PTP, Active.Line, Active.Circle, Active.Stop constitute an Active state Set, Active node transition rule is effective to eight sub- states.For example, to any one in eight states, write-in " e_ready " event, is transferred to Ready states from current state by the state of controller state machine and (waits instruction input shape State).
Furthermore it is also possible to using Lua language, write the startup file of top control module, the startup file be arranged to carry out as Lower action:
A) by OROCOS import methods, load-on module is run;
B) refreshing frequency of definition module, the priority level of thread;
C) assignment is carried out to the attribute of module;
D) by OROCOS connect methods, by the input of top control module, output interface and algoritic module and communication tube The interface for managing module sets up connection.
E) by OROCOS start methods, top control module is run, top control module will first call StartHook () function, Then default refreshing frequency is pressed, in real time periodically invoked UpdateHook () function.
2nd, for algoritic module:
Algoritic module creates ROS Package using ROS orocreate-catkin-pkg methods, is designated as Ec_ Control_loop, then in Package, by the RTT for inheriting OROCOS::TaskContext classes, are designated as Ec_ control_loop_component。
(1) in the constructed fuction of Ec_control_loop_component classes, it is arranged to carry out following operation:
A) algoritic module utilizes OROCOS RTT::Input and RTT::Output methods, to input, output interface is carried out Definition.
The interface wherein inputted includes:
1. incoming motor operation data of communication management module;
2. the incoming diagnostic data of communication management module;
3. the state of equipment state machine;
4. the state of controller state machine;
The interface of output includes:
1. motor control instruction data, are exported to device communication module;
2. equipment state machine event is triggered, and is exported and is given equipment state machine;
3. controller state machine event is triggered, and is exported and is given controller state machine.
B) function call interface is set using OROCOS Operational Caller methods, what setting event was reported connects Mouthful:The interface handles the event report for triggering top control module in the return origin instruction response letter of the various motion plannings of function setup Number.
C) OROCOS Properties methods are called to define the attribute of algoritic module, algoritic module defines a mechanical arm The attribute of joint number.
(2) in Ec_control_loop_component StartHook () member function, it is arranged to carry out as follows Operation:
A) whether audit log report is normal, if abnormal directly exit, and relevant information is passed by event reporting interface Pass top control module processing;
B) check whether motor operation data channel there are data, if no data is directly exited, and relevant information is passed through into thing Part reporting interface passes to top control module processing.
(3) for UpdateHook () member function of Ec_control_loop_component classes, the function is set to exist When algoritic module is run, the frequency real time execution (being such as set to 100Hz) set according to user could be arranged to perform following behaviour Make:
A) Read Controller state machine state;
B) according to the different conditions of controller state machine, different operating is performed:
Ith, if pause motion, pause motion, pause motion, manual teaching, jerk returns to origin state.Now, hold The following operation of row:
If the control instruction number of instruction buffer is less than 20, all instructions are sent jointly into telecommunication management mould Block, and be wait instruction input state by the state change of controller state machine;
If the control instruction number of instruction buffer is more than 20,20 of instruction fetch queue end, it is sent to logical Believe management module;
IIth, if halted state, then do nothing.
(4) for Ec_control_loop_component classes, pause motion, pause motion, pause motion, hand are defined Dynamic teaching, jerk returns to origin, suspends, the function call interface such as recovery, realizes as follows:
A) pause motion, pause motion, pause motion, manual teaching returns to the function of origin, and inside is realized as follows:
Check controller state machine whether in wait instruction input state.If not then exiting, and by relevant information Top control module is passed to by event reporting interface to handle;
Read the current status information of motor;
According to the current state of motor, pause motion is called respectively, pause motion, pause motion, manual teaching returns to original The motion planning of point, and the motor control instruction of generation is saved in instruction buffer;
Controller state machine is set to corresponding state.Such as pause motion call back function, then set controller state machine For pause motion state.
B) function is suspended, inside is realized as follows:
Check whether controller state machine is pause motion, and pause motion, pause motion, manual teaching returns to origin etc. State.If not then exiting, and relevant information is passed into top control module by event reporting interface handled;
The current state of current controller state machine is recorded, and the state of controller state machine is changed into halted state.
C) reconstruction, inside is realized as follows:
Check whether controller state machine is halted state.If not then exiting, and relevant information is passed through into event report Accuse interface and pass to top control module processing;
The state of controller state machine is changed into the state before pause.
D) jerk function, inside is realized as follows:
Ith, check whether controller state machine is pause motion, and pause motion, pause motion, manual teaching returns to origin Etc. state.If not then exiting, and relevant information is passed into top control module by event reporting interface handled;
IIth, the current status information of motor is read;
IIIth, motor control instruction buffering area is reset;
IVth, speed planning motion planning is called, motor is allowed with the shortest time, speed is reduced to 0, and by the motor control of generation Instruction is saved in instruction buffer.
(5) Lua language is used, the startup file of algoritic module is write, following action is arranged to carry out:
A) OROCOS import methods, loading algorithm module are passed through;
D) refreshing frequency of algoritic module, the priority level of thread are defined;
C) assignment is carried out to the attribute of algoritic module;
D) by OROCOS connect methods, by the input of algoritic module, output interface and top control module and communication tube The interface for managing module sets up connection.
E) by OROCOS start methods, algoritic module is run, algoritic module first calls StartHook () function, so The refreshing frequency set is pressed afterwards, in real time periodically invoked UpdateHook () function.
3rd, for communication management module:
Communication management module can develop board communications, Ke Yi by the ttyACM0 in linux main frames minicom and Arm A CANOpen master station protocol is run on the Arm development boards, the master station protocol can set an instruction buffer area, at most may be used To store 25 instructions.
Communication management module can utilize OROCOS RTT::Input and RTT::Output methods and robot algorithm mould Block and top control module are communicated.
Equipment state machine is set up using rFSM softwares, the service logic to communication management module is controlled.
Communication management module utilizes OROCOS RTT::Input and RTT::Output method and apparatus state machine is connected, The state of equipment state machine, and reading state can be changed.
(1) communication module is created as ROS Package, Ran Hou using ROS orocreate-catkin-pkg methods In Package, by the RTT for inheriting OROCOS::TaskContext classes, create OROCOS Real time capable module, are designated as Ec_ component。
In the constructed fuction of Ec_component classes, following operation is arranged to carry out:
A) communication management module utilizes OROCOS RTT::Input and RTT::Output methods, to input, output interface It is defined.
The interface wherein inputted includes:
1. incoming control instruction data of algoritic module;
2. the state of equipment state machine;
The interface of output includes:
1. diagnostic data, is exported to algoritic module and top control module;
2. motor operation data and mechanical arm status data, are exported to algoritic module;
3. state machine events are triggered, and are exported and are given equipment state machine.
B) using OROCOS Operational Caller method defined function calling interfaces, communication management module definition The interface of event report, the event report processing function of top control module is triggered by the interface.
C) OROCOS Properties methods are called to define the attribute of communication management module, communication management module defines one The attribute of individual joint of mechanical arm number.
(2) in Ec_component StartHook () member function, it is arranged to carry out following operation:
A) whether audit log report is normal, if abnormal directly exit, and relevant information is passed by event reporting interface Top control module is passed to be handled;
B) motor driving initialization:
Ith, set up and communicated with motor driver by ttyACM0;
IIth, motor command buffering queue is emptied;
IIIth, motor is enabled, if enabled successfully, is carried out next step, is otherwise exited;
IVth, the position of motor, the mechanical arm current state of calculating robot, including joint angles, mechanical arm tail end are read Pose;
C) mechanical arm state initialization:
According to motor position, judge whether mechanical arm needs to perform back to zero motion.If any joint angles of mechanical arm with Zero degree differs by more than 0.01 degree, then performs back to zero motion, call pause motion to plan, back to zero motion is planned.
D) state of equipment state machine is changed:
If the mechanical arm of robot needs to perform back to zero motion, control device state machine keeps Init states constant; Otherwise, " e_nominal " event is sent to equipment state machine, is Active.Nominal by the State Transferring of equipment state machine.
(3) for Ec_component UpdateHook () member function, the function is set to be transported in communication management module During row, the frequency real time execution (being such as set to 1KHz) set with user is arranged to carry out following operation:
A) equipment state machine state is read;
B) according to the different conditions of equipment state machine, different operating is performed:
Ith, if Init states, the back to zero motion of mechanical arm is performed.Now, following operation is performed:
The clock of reading system, according to back to zero Motion trajectory result, calculates the movement instruction of the moment motor, and will The wall scroll movement instruction is sent to CANOpen main websites.
If motion back to zero success, sends " e_nominal " event to equipment state machine, equipment state machine is converted to Active.Nominal, and the event is reported to top control module.
IIth, if Active.Nominal states.Now, following operation is performed:
From the input channel of control instruction data, control instruction is read, and store into motor command buffering queue.
The existing number of instructions of CANOpen main websites instruction buffer is read, if less than 10, is then disposably referred to from motor Make and taken out in buffering queue 15 movement instructions be sent to CANOpen main websites.If the number of instructions of motor command buffering queue It is less than 15, then disposable to be all sent to CANOpen main websites.
IIIth, if Active.Recovery states.Now, communication management module is in recovery state.
Now, according to diagnostic message, system mode is recovered, if being successfully recovered, " e_ is sent to equipment state machine Nominal " events, Active.Nominal is converted to by equipment state machine.And report the event to top control module.
If recovery is unsuccessful, " e_fault " event is sent to state machine, state machine is converted to Fault states, by the event Report to top control module, and directly exit UpdateHook ().
IVth, if Active.Halt states.Now, module placed in a suspend state, performs following operation:Check control Whether there is new instruction in the input channel of director data, if there is new command, read control instruction, and store to motor command In buffering queue.
Vth, if Active.Hands states.Now, module is in manual mode, performs following operation:
The clock of reading system, according to Motion trajectory result, calculates the movement instruction of the moment motor, and by the list Bar movement instruction is sent to CANOpen main websites.
VIth, if Fault states, then UpdateHook () is directly exited.
C) motor status are read, according to the normatron tool shoulder joint and end movement status information of mechanical arm, and By output data passage, algoritic module and top control module are passed to;
D) the whether wrong report information of control main website is checked, if error message, then diagnostic message calculation is passed into Method module and top control module." e_recovery " event is sent to equipment state machine, equipment state machine is converted to Active.Recovery states simultaneously report the event to top control module.
(4) for Ec_component CleanUpHook () member function, the function is when module terminates operation, certainly It is dynamic to call, set and perform following operation:
A) motor driving is closed to enable;
B) motor drive connection is closed.
(5) with reference to shown in Fig. 8, Fig. 8 is the state change schematic diagram of equipment state machine.Shared Init (initialization), Fault (interruption), Active.Recovery (recovery), Active.Hands (manual teaching), Active.Halt (pause), Six states of Active.Nominal (operating).Active.Recovery、Active.Hands、Active.Halt、 Tetra- states of Active.Nominal constitute the state set of an Active (enable), and Active node transition rule is right Four sub- states are effective.
(6) Lua language is used, the startup file of the module is write, the startup file, which is set, performs following action:
A) by OROCOS import methods, communication management module is loaded;
B) refreshing frequency of communication management module, the priority level of thread are defined;
C) assignment is carried out to the attribute of communication management module;
D) by OROCOS connect methods, by the input of communication management module, output interface and top control module and calculation The interface of method mould etc. sets up connection.
E) by OROCOS start methods, communication management module is run, communication management module first calls StartHook () function, then good refreshing frequency by definition, in real time periodically invoked UpdateHook () function.
For above-mentioned top control module, algoritic module and communication management module, it is set to after operation, if during user needs Way stops the module, while the ctrl keys of keypad and D keys.
To sum up embodiment, based on ROS and OROCOS, utilizes OROCOS real time characteristic, it is ensured that software program it is real-time Performance;ROS opening is made full use of, top control module, algoritic module and the communication management module developed based on ROS, OROCOS Real-time Communication for Power is carried out, a complete robot controller software is collectively formed;It further established controller state machine and set Standby state machine, is effectively managed the service logic of top control module, communication management module so as to realize.
Pass through OROCOS RTT::Input,RTT::Output methods set up top control module, algoritic module and telecommunication management The data input of module, output channel, by OROCOS Operational Caller method defined function calling interfaces, lead to The Properties methods for crossing OROCOS define the attribute of top control module, algoritic module and communication management module.Thus it is guaranteed that Independence and decoupling between top control module, algoritic module and communication management module.
Each technical characteristic of embodiment described above can be combined arbitrarily, to make description succinct, not to above-mentioned reality Apply all possible combination of each technical characteristic in example to be all described, as long as however, the combination of these technical characteristics is not deposited In contradiction, the scope of this specification record is all considered to be.
Embodiment described above only expresses the several embodiments of the present invention, and it describes more specific and detailed, but simultaneously Can not therefore it be construed as limiting the scope of the patent.It should be pointed out that coming for one of ordinary skill in the art Say, without departing from the inventive concept of the premise, various modifications and improvements can be made, these belong to the protection of the present invention Scope.Therefore, the protection domain of patent of the present invention should be determined by the appended claims.

Claims (10)

1. a kind of robot pause motion control method, it is characterised in that including:
Receive the pause motion instruction of control end transmission;
Current robot be in point-to-point move, linear motion, circular motion or return to origin motion any one is current Under motion state, the pause motion instruction is performed;
The target point of current kinetic and the remaining time of motion are obtained, the remaining time of the target point and motion is saved as extensive The trajectory calculation parameter of multiple current kinetic;Current location, present speed and the current acceleration of mechanical arm tail end are read, and is calculated The pause motion track that mechanical arm tail end is passed through when changing to stopping by current location, present speed and current acceleration;
When calculating machine arm end is run on the pause motion track, joint of mechanical arm each position angle on target, Target angular velocity and target angular acceleration are forwarded to control main website.
2. robot pause motion control method according to claim 1, it is characterised in that if the current kinetic is point Origin motion is moved or returns to,
The target point for obtaining and recording current kinetic and the remaining time of motion, and read the present bit of mechanical arm tail end Put, present speed and current acceleration, and pause motion rail is calculated according to the current location, present speed and current acceleration The method of mark comprises the following steps:
Calculate current point-to-point or return to the target point of origin motion, undergone according to the total duration of current kinetic and current kinetic Time Calculation current kinetic remaining time, record the target point and remaining time;
Read the current joint angles of mechanical arm, joint angular speed, and joint angular acceleration;
Speed is reduced to 0 principle according to peak acceleration, calculate the time that each joint velocity is reduced to 0, be designated as operation suspension Time for each instruction;
Operation suspension track is obtained according to operation suspension time for each instruction.
The method of the angle on target in each joint of calculating machine arm, target angular velocity and target angular acceleration includes following step Suddenly:
Each joint of mechanical arm is calculated according to the current joint angular speed of mechanical arm and the operation suspension time for each instruction Target angular acceleration;
The target angular velocity and joint angle on target of each joint of mechanical arm are calculated according to the target angular acceleration.
3. robot pause motion control method according to claim 2, it is characterised in that the current point-to-point is returned The target point θ moved to origin1, the remaining time of current kinetic:Tr=T0-t0, wherein T0It is the total duration of current kinetic, t0It is The time that current kinetic has undergone;
The Time Calculation formula that each described joint velocity is reduced to 0 is:
T i = θ · i 0 θ ·· i m a x
Wherein, i represents the label in joint, chooses TiIn maximum, be designated as operation suspension time for each instruction T;
The target angular acceleration of joint of mechanical armComputing formula is:
θ ·· i m a x = θ · i 0 T
The target angular velocity of each joint of mechanical armWith joint angle on target θiComputing formula be respectively:
θ · i = θ · i 0 - θ ·· i m a x t
θ i = θ · i 0 t - θ ·· i m a x t 2 2
In formula, θ0For the current joint angles of mechanical arm,For joint angular speed,For joint angular acceleration;T represents pause fortune At the time of during dynamic.
4. robot pause motion control method according to claim 1, it is characterised in that if the current kinetic is straight Line is moved;
The target point for obtaining and recording current kinetic and the remaining time of motion, and read the present bit of mechanical arm tail end Put, present speed and current acceleration, and pause motion rail is calculated according to the current location, present speed and current acceleration The method of mark includes:
The target point currently moved along a straight line is calculated, the Time Calculation undergone according to the total duration of current kinetic and current kinetic is worked as The remaining time of preceding motion, record the target point and remaining time;
According to the track at straight-line trajectory calculating machine arm current time, joint angular speed and joint angular acceleration;
According to the track, the track that joint angular speed and joint angle acceleration calculation pause linear motion needs are passed by is obtained Pause motion track, and record the track that mechanical arm has been passed by along straight line when recovering motion;
The method of the angle on target in each joint of calculating machine arm, target angular velocity and target angular acceleration includes following step Suddenly:
Calculate track position, path velocity and the path acceleration of mechanical arm any instant t in pause motion:
According to target location, speed and the acceleration of the track position, path velocity and path acceleration calculating machine arm end Degree;
Using inverse kinematics method according to the target location, the angle on target of velocity and acceleration calculating machine shoulder joint, mesh Mark angular speed and target angular acceleration.
5. robot pause motion control method according to claim 4, it is characterised in that described currently to move along a straight line Target point se, the remaining time of current kinetic:Tr=T0-t0, wherein, T0It is the total duration of current kinetic, t0Be current kinetic The time of experience;
Pause linear motion needs the trajectory calculation formula passed by be:
s 1 = s · 0 2 2 a m a x
Wherein, amaxFor the acceleration that linear motion is maximum;Record s2=s0+s1, wherein, s2It is straight to recover mechanical arm edge during motion The track that line has been passed by;
Any instant t track position, path velocity and path acceleration:
s = s · 0 t - a m a x t 2 2
s · ( t ) = s · 0 - a m a x t
s ·· ( t ) = a m a x
Wherein, s0For the track at mechanical arm current time,For joint angular speed,For joint angular acceleration, s is track position,For path velocity,For path acceleration;
The target location of mechanical arm tail end, the computing formula of velocity and acceleration are:
Wherein, PtFor target location,For speed,For acceleration,It is the unit vector currently moved along a straight line;
The computing formula of the angle on target of the joint of mechanical arm, target angular velocity and target angular acceleration is:
θt=invKinematics (Pt)
θ · t = J + P · t
θ ·· t = J + ( p ·· t - J · θ · t )
Wherein, θtFor angle on target,For target angular velocity,For target angular acceleration, invKinematics () represents inverse fortune The dynamic computing formula learned, J is Jacobian matrix,Led for the single order of Jacobian matrix, J+For pseudoinverse.
6. robot pause motion control method according to claim 1, it is characterised in that if the current kinetic is circle Arc is moved;
The target point for obtaining and recording current kinetic and the remaining time of motion, and read the present bit of mechanical arm tail end Put, present speed and current acceleration, and pause motion rail is calculated according to the current location, present speed and current acceleration The method of mark includes:
The target point of current circular motion is calculated, the Time Calculation undergone according to the total duration of current kinetic and current kinetic is worked as The remaining time of preceding motion, record the target point and remaining time;
According to the track at circular motion trajectory calculation mechanical arm current time, joint angular speed and joint angular acceleration;
According to the track, the track that joint angular speed and joint angle acceleration calculation pause circular motion needs are passed by is obtained Operation suspension track, and record the track that mechanical arm has been passed by along circular arc when recovering motion;
The method of the angle on target in each joint of calculating machine arm, target angular velocity and target angular acceleration includes following step Suddenly:
Calculate track position, path velocity and the path acceleration of mechanical arm any instant t in pause motion;
According to target location, speed and the acceleration of the track position, path velocity and path acceleration calculating machine arm end Degree;
Using inverse kinematics method according to the target location, the angle on target of velocity and acceleration calculating machine shoulder joint, mesh Mark angular speed and target angular acceleration.
7. robot pause motion control method according to claim 6, it is characterised in that the current circular motion Target point se, the remaining time of current kinetic:Tr=T0-t0, wherein, T0It is the total duration of current kinetic, t0Be current kinetic The time of experience;
Pause circular motion needs the trajectory calculation formula passed by be:
s 1 = s · 0 2 2 a m a x
Wherein, amaxFor the acceleration that circular motion is maximum;Record s2=s0+s1, wherein, s2To recover mechanical arm edge circle during motion The track that arc has been passed by;
Any instant t track position, path velocity and path acceleration:
s = s · 0 t - a m a x t 2 2
s · ( t ) = s · 0 - a max t
s ·· ( t ) = a m a x
Wherein, s0For the track at mechanical arm current time,For joint angular speed,For joint angular acceleration, s is track position,For path velocity,For path acceleration;
The target location P of mechanical arm tail endt=[xt,yt,zt]T
x t = x c + R cos ( θ t ) a 1 + R sin ( θ t ) b 1 y t = y c + R cos ( θ t ) a 2 + R sin ( θ t ) b 2 z t = z c + R cos ( θ t ) a 3 + R sin ( θ t ) b 3
Wherein, θtFor circular arc P0PtCorresponding central angle, meetsa1,a2,a3,b1,b2,b3, the parameter such as R is current circle Arc movement locus known parameters;
The target velocity of mechanical arm tail endAnd accelerationComputing formula is:
Wherein,It is point PtTangent vector;
The computing formula of the angle on target of the joint of mechanical arm, target angular velocity and target angular acceleration is:
θt=invKinematics (Pt)
θ · t = J + P · t
θ ·· t = J + ( p ·· t - J · θ · t )
Wherein, θtFor angle on target,For target angular velocity,For target angular acceleration, invKinematics () represents inverse fortune The dynamic computing formula learned, J is Jacobian matrix,Led for the single order of Jacobian matrix, J+For pseudoinverse.
8. the robot pause motion control method according to any one of claim 1 to 7, it is characterised in that also include:
After pause motion instruction is performed, asynchronous triggering pause motion performs function and calls pause motion by first interface Plan function;Wherein, the first interface is based on the real-time input/output interface that OROCOS is created on ROS;
The pause motion planning function is performed, and calculates pause motion track;
And be sent to the angle on target, target angular velocity and target angular acceleration of the joint of mechanical arm by second interface Equipment communication software forward is to controlling main website;Wherein, the second interface be based on ROS create OROCOS real-time input/ Output interface.
9. robot pause motion control method according to claim 8, it is characterised in that the mesh of calculating machine shoulder joint The method of mark angle, target angular velocity and target angular acceleration includes:
After mechanical arm starts to move on the pause motion track, the run duration t of mechanical arm is countedn
If the run duration is met:tn≤ T, angle on target, the mesh in each joint of one-time mechanical arm are calculated every setting time τ Mark angular speed and target angular acceleration;If run duration tnMeet:tn> T, then stop motion;
Wherein, T is operation suspension time for each instruction.
10. a kind of robot pause motion control system, it is characterised in that including:Top control module, algoritic module and telecommunication management Module;
The top control module, the pause motion instruction for receiving control end transmission;Current robot be in point-to-point motion, Linear motion, circular motion are returned under any one current motion state of origin motion, perform the pause motion instruction;
The algoritic module, for obtaining the target point of current kinetic and the remaining time of motion, by the target point and motion Remaining time save as recover current kinetic trajectory calculation parameter;Read current location, the present speed of mechanical arm tail end And current acceleration, and calculating machine arm end passed through when changing to stopping by current location, present speed and current acceleration The pause motion track crossed;When calculating machine arm end is run on the pause motion track, joint of mechanical arm is in each position Angle on target, target angular velocity and the target angular acceleration put;
The communication management module, for the angle on target, target angular velocity and target angular acceleration to be forwarded into control master Stand.
CN201710256933.0A 2017-04-19 2017-04-19 Robot pause motion control method and system Active CN106945044B (en)

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 true CN106945044A (en) 2017-07-14
CN106945044B 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)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109822230A (en) * 2019-01-31 2019-05-31 大族激光科技产业集团股份有限公司 Motion control method, device and the computer equipment of laser cutting nozzle
CN110861083A (en) * 2019-10-25 2020-03-06 广东省智能制造研究所 Robot teaching method and device, storage medium and robot
CN111015658A (en) * 2019-12-20 2020-04-17 深圳市优必选科技股份有限公司 Method and device for controlling robot
CN112497210A (en) * 2020-11-09 2021-03-16 北京配天技术有限公司 Robot movement method, robot and device with storage function
CN112799325A (en) * 2020-12-31 2021-05-14 泉芯集成电路制造(济南)有限公司 Method and device for monitoring motion of mechanical arm and related equipment
CN114102597A (en) * 2021-11-30 2022-03-01 伯朗特机器人股份有限公司 Method for controlling inching operation of mechanical arm joint, electronic device and storage medium

Citations (5)

* Cited by examiner, † Cited by third party
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

Patent Citations (5)

* Cited by examiner, † Cited by third party
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

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109822230A (en) * 2019-01-31 2019-05-31 大族激光科技产业集团股份有限公司 Motion control method, device and the computer equipment of laser cutting nozzle
CN109822230B (en) * 2019-01-31 2021-12-14 大族激光科技产业集团股份有限公司 Motion control method and device of laser cutting nozzle and computer equipment
CN110861083A (en) * 2019-10-25 2020-03-06 广东省智能制造研究所 Robot teaching method and device, storage medium and robot
CN110861083B (en) * 2019-10-25 2020-11-24 广东省智能制造研究所 Robot teaching method and device, storage medium and robot
CN111015658A (en) * 2019-12-20 2020-04-17 深圳市优必选科技股份有限公司 Method and device for controlling robot
CN111015658B (en) * 2019-12-20 2021-06-29 深圳市优必选科技股份有限公司 Method and device for controlling robot
CN112497210A (en) * 2020-11-09 2021-03-16 北京配天技术有限公司 Robot movement method, robot and device with storage function
CN112799325A (en) * 2020-12-31 2021-05-14 泉芯集成电路制造(济南)有限公司 Method and device for monitoring motion of mechanical arm and related equipment
CN114102597A (en) * 2021-11-30 2022-03-01 伯朗特机器人股份有限公司 Method for controlling inching operation of mechanical arm joint, electronic device and storage medium

Also Published As

Publication number Publication date
CN106945044B (en) 2019-12-17

Similar Documents

Publication Publication Date Title
CN106945044A (en) Robot pause motion control method and system
CN106926243A (en) Robot teaching motion control method and system
CN106950924A (en) Robot circular motion control method and system
CN106914900B (en) Robot control system based on ROS and OROCOS
CN106985140B (en) Robot point-to-point motion control method and system
CN110733038B (en) Industrial robot remote monitoring and data processing system
CN102581850B (en) GSK-Link bus based modular robot control device and control method
CN106959694A (en) robot linear motion control method and system
CN107030695A (en) Robot returns to origin motion control method and system
CN102195845B (en) Method, device and equipment for realizing active-standby switching of main control board
CN106945045B (en) communication method and system for robot control based on ROS and OROCOS
CN106060058A (en) Internet-based industrial robot remote control method
CN109347884B (en) Method and device for converting real-time Ethernet to field bus and storage medium
CN104281086A (en) Soft PLC control system based on EtherCAT communication protocol and applied to automatic argon-welding device
CN109514555A (en) Command interaction method, robot demonstrator, robot controller and storage medium
CN107526681B (en) Robot test system and test method
US20130315362A1 (en) Nuclear digital instrumentation and control system
CN102176122A (en) Automatic control system based on finite automaton and control method thereof
CN106956265A (en) The execution method and system of robot motion planning based on ROS and OROCOS
CN106965181A (en) Robot control method and system based on ROS and OROCOS
CN109814493A (en) A kind of control method, control system and the control device of rotating-disk testing sorting machine
CN109454641A (en) A kind of motion controller multitask division of labor and data interactive method
Knoop et al. A CORBA-based distributed software architecture for control of service robots
CN102819508B (en) Method for responding to signals from security monitoring subsystem
Dantam et al. Multiprocess communication and control software for humanoid robots

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