CN106926243B - Robot teaching motion control method and system - Google Patents

Robot teaching motion control method and system Download PDF

Info

Publication number
CN106926243B
CN106926243B CN201710256934.5A CN201710256934A CN106926243B CN 106926243 B CN106926243 B CN 106926243B CN 201710256934 A CN201710256934 A CN 201710256934A CN 106926243 B CN106926243 B CN 106926243B
Authority
CN
China
Prior art keywords
teaching
motion
target angular
teaching motion
planning
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
Application number
CN201710256934.5A
Other languages
Chinese (zh)
Other versions
CN106926243A (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 CN201710256934.5A priority Critical patent/CN106926243B/en
Publication of CN106926243A publication Critical patent/CN106926243A/en
Application granted granted Critical
Publication of CN106926243B publication Critical patent/CN106926243B/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/1656Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05BCONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00Program-control systems
    • G05B2219/30Nc systems
    • G05B2219/40Robotics, robotics mapping to robotics vision
    • G05B2219/40519Motion, trajectory planning

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Numerical Control (AREA)

Abstract

The invention relates to a robot teaching motion control method and a system, wherein the method comprises the following steps: receiving a teaching motion instruction transmitted by a control end; wherein the teaching motion command comprises a starting point and a motion direction; calculating the edge position of a working area in the direction according to the starting point and the moving direction of the tail end of the mechanical arm, taking the point on the edge position as a motion terminal point, and calling a corresponding speed planning algorithm to plan a teaching motion track according to the state of the teaching motion instruction and the motion terminal point; and calculating the target angle, the target angular velocity and the target angular acceleration of each position of each joint of the mechanical arm when the joints run on the teaching motion trail, and sending the target angle, the target angular velocity and the target angular acceleration to equipment communication software for recording. The technology of the invention can form a complete robot teaching motion control system, reduce the development cost of the robot control system and improve the control effect.

Description

Robot teaching motion control method and system
Technical field
The present invention relates to technical field of robot control, more particularly to a kind of robot teaching motion control method and are System.
Background technique
Robot Operating System (ROS) is the robot operating system of open source, can be robot development person One standardized, open source programming framework is provided.But ROS does not support real-time thread to operate at present.Open Robot Control Software (OROCOS) is also a kind of robot control software's programming framework of open source, and its feature is to support in fact When threading operation, but its opening, versatility do not have ROS good.
Therefore, robot teaching movement is a kind of important motion mode of robot, at present in some application schemes, OROCOS is used on ROS, but it is soft cannot to constitute a complete robot controller on the framework built for prior art Part is unable to fully the characteristic using ROS and OROCOS when executing teaching campaign, and system development costs are high, and control effect is poor.
Summary of the invention
Based on this, it is necessary in view of the above technical problems, provide a kind of robot teaching motion control method, reduce system Development cost improves control effect.
A kind of robot teaching motion control method, comprising:
Receive the teaching movement instruction of control terminal transmission;Wherein, the teaching movement instruction includes starting point and movement side To;
Working region marginal position in this direction is calculated according to the starting point of mechanical arm tail end and moving direction, and will Point on the marginal position is called as exercise end according to the state of the teaching movement instruction and the exercise end Corresponding velocity planning algorithm plans teaching motion profile;
Target angle, the target angle of each joint of calculating machine arm each position when being run on the teaching motion profile Speed and target angular acceleration, and the target angle, target angular velocity and target angular acceleration are sent to equipment communication software It is recorded.
A kind of robot teaching kinetic control system, comprising: top control module, algoritic module and communication management module;
The top control module, for receiving the teaching movement instruction of control terminal transmission;Wherein, the teaching movement instruction packet Include starting point and the direction of motion;
The algoritic module, for calculating work in this direction according to the starting point and moving direction of mechanical arm tail end Edges of regions position, and using the point on the marginal position as exercise end, according to the state of the teaching movement instruction and The exercise end calls corresponding velocity planning algorithm planning teaching motion profile;The each joint of calculating machine arm is described Target angle, target angular velocity and the target angular acceleration of each position when being run on teaching motion profile, and by the target angle Degree, target angular velocity and target angular acceleration;
The communication management module, for being recorded to the target angle, target angular velocity and target angular acceleration.
Above-mentioned robot teaching motion control method and system, after receiving the teaching movement instruction that control terminal transmits, according to Teaching movement instruction calculates the motion profile of teaching, and the mesh that each joint of real-time computer tool arm is run on the motion profile The state parameters such as angle, target angular velocity and target angular acceleration are marked, are carried out above-mentioned state parameter by equipment communication software Record realizes the teaching motion control process to robot;The technical solution may be constructed a complete robot teaching fortune Autocontrol system reduces robot control system development cost, improves control effect.
Detailed description of the invention
Fig. 1 is the robot teaching motion control method flow chart of the embodiment of the present invention;
Fig. 2 is robot teaching kinetic control system structural schematic diagram;
Fig. 3 is that top control module executes algorithm flow chart;
Fig. 4 is that algoritic module executes algorithm flow chart;
Fig. 5 is the stroke of classical S type speed planning, speed, acceleration, the derivative curve figure of acceleration;
Fig. 6 is the parameter calculation flow chart of S type speed planning;
Fig. 7 is S type velocity planning algorithm whole flow process figure;
Fig. 8 is to stop S type velocity planning algorithm flow chart in real time;
Fig. 9 is the hardware structure model of the robot control system of an application example;
Figure 10 is the architecture diagram built based on ROS and OROCOS;
Figure 11 is the state change schematic diagram of controller state machine;
Figure 12 is the state change schematic diagram of equipment state machine.
Specific embodiment
The embodiment of robot teaching motion control method of the invention is illustrated with reference to the accompanying drawing.
In the embodiment of the present invention, the teaching campaign refers to that user inputs starting point and moving direction, mechanical arm edge always The direction moves always, until user assigns switching moving direction or has arrived at side working region edge, in the process In, record the movement instruction at each moment of mechanical arm.
Refering to what is shown in Fig. 1, Fig. 1 is the robot teaching motion control method flow chart of the embodiment of the present invention, comprising:
S10 receives the teaching movement instruction of control terminal transmission;Wherein, the teaching movement instruction includes starting point and fortune Dynamic direction;
In above-mentioned steps, it can use preset communication protocol and receive teaching in a manner of asynchronous remote procedure call Movement instruction;For example, the teaching movement instruction includes starting point θ0And the direction of motion;
In the process, it can be operator and teaching movement instruction generated by human-computer interaction interface, the instruction is without passing Pass parameter;By preset communication protocol, it is such as based on IEC (The Internet Communications Engine, internet Communication engines) exploitation communication protocol, in a manner of asynchronous remote procedure call from human-computer interaction interface receive teaching campaign refer to It enables.
S20 calculates working region marginal position in this direction according to the starting point of mechanical arm tail end and moving direction, And using the point on the marginal position as exercise end, according to the state of the teaching movement instruction and the exercise end, Call corresponding velocity planning algorithm planning teaching motion profile;
In one embodiment, after receiving the teaching movement instruction, asynchronous triggering teaching Motor execution function is simultaneously Teaching motion planning function is called by first interface;Wherein, the first interface be based on ROS create OROCOS it is real-time Input/output interface;
Further, before calling teaching motion planning function, controller state machine is judged by teaching Motor execution function It whether is to prepare (Ready) state;If so, teaching campaign described in the Operational Caller method call for passing through OROCOS It plans function, and controller state machine-cut is changed to teaching campaign (Active.Hands) state of execution;If it is not, then refusing to execute This time instruct;Wherein, the controller state machine is changed state, and reading state, be equipped with initialization, instruction waits, Corresponding state is interrupted and enabled to instruction execution.
The teaching motion planning function is further executed, is calculated in this direction according to starting point and moving direction Working region marginal position, and using the point on the marginal position as exercise end;After calling teaching motion planning function, root Teaching motion planning process is executed according to the teaching motion planning function, and checks whether controller state machine is to execute teaching fortune Dynamic state;If so, the step of executing the motion profile for calculating teaching, otherwise, exits execution process.
S30, target angle, the mesh of each joint of calculating machine arm each position when being run on the teaching motion profile Angular speed and target angular acceleration are marked, and the target angle, target angular velocity and target angular acceleration are sent to equipment communication Software is recorded.
Specifically, planning teaching motion profile, and calculating machine according to the teaching movement instruction and the exercise end Target angle, target angular velocity and the target angle for each position that each joint of arm is run on the teaching motion profile accelerate The target angle, target angular velocity and target angular acceleration are sent to equipment communication software by second interface and carried out by degree Record;Wherein, the second interface is based on the real-time input/output interface for creating OROCOS on ROS;
In one embodiment, if inputted currently without new command, and mechanical arm does not move to boundary;
S type velocity planning algorithm is then called to plan teaching motion profile;A machine is calculated separately every setting time τ foundation Target angle, target angular velocity and the target angular acceleration in each joint of tool arm;
The target angle, target angular velocity and target angular acceleration are sent to equipment communication software;
Equipment communication software is recorded.
As another embodiment, if receiving halt instruction or mechanical arm arrived boundary;
It then calls and stops S type velocity planning algorithm planning teaching motion profile in real time;It is counted respectively every setting time τ foundation Calculate target angle, target angular velocity and the target angular acceleration in each joint of one-time mechanical arm;
The target angle, target angular velocity and target angular acceleration are sent to equipment communication software;
Equipment communication software is recorded.
Further, change direction instruction if receiving;
It then calls and stops S type velocity planning algorithm planning teaching motion profile in real time;It is counted respectively every setting time τ foundation Calculate target angle, target angular velocity and the target angular acceleration in each joint of one-time mechanical arm;Until mechanical arm stops;
After mechanical arm stopping, using current point as starting point, workspace in this direction is calculated according to moving direction Domain marginal position, and using the point as terminal, recalculate working region marginal position.
In above-mentioned planning teaching motion profile, S type speed planning can be stopped using S type velocity planning algorithm and in real time and calculated Method, arithmetic analysis are as follows:
(1) stroke of the S type velocity planning algorithm includes three phases:
Boost phase: acceleration can increase linearly at a constant ratio maximum value since 0, be then lowered into 0;
Constant velocity stage: remain a constant speed movement;
Decelerating phase: it is at the uniform velocity reduced speed now by described, final speed and acceleration return to 0.
(2) the real-time stopping S type velocity planning algorithm process includes:
When mechanical arm runs to t moment, it is steady to cook up one section of mechanical arm under current running status by 0 < t < T Operation and fastest decelerating phase;T is teaching motion profile run duration.
The technical solution of above-described embodiment, using the real-time input/output interface of OROCOS, by setting communication protocol with The mode of asynchronous remote procedure call receives teaching movement instruction, and starting teaching movement instruction triggers function, calls robot fortune The teaching Motor execution function of dynamic planning calculates the motion profile of teaching, and each pass of real-time computer tool arm according to the function The state parameters such as target angle, target angular velocity and the target angular acceleration run on the motion profile are saved, it is logical by equipment Letter software records above-mentioned state parameter, realizes the teaching motion control process to robot;The technical solution can be into The functions such as reception, the parsing of row control instruction, algorithm are called, executed, and parameter is calculated and transmitted in real time, may be constructed one completely Robot teaching kinetic control system, reduce robot control system development cost, improve control effect.In addition combined with control The application of device state machine and flag bit processed realizes the optimal control to algorithm process process, further improves control effect.
It is directed to robot teaching motion control method, the present invention provides the corresponding robot teaching fortune of the control method Autocontrol system,
Refering to what is shown in Fig. 2, Fig. 2 is robot teaching kinetic control system structural schematic diagram, comprising: top control module, algorithm Module and communication management module;
The top control module, for receiving the teaching movement instruction of control terminal transmission;Wherein, the teaching movement instruction packet Include starting point and the direction of motion;
The algoritic module, for calculating work in this direction according to the starting point and moving direction of mechanical arm tail end Edges of regions position, and using the point on the marginal position as exercise end, according to the state of the teaching movement instruction and The exercise end calls corresponding velocity planning algorithm planning teaching motion profile;The each joint of calculating machine arm is described Target angle, target angular velocity and the target angular acceleration of each position when being run on teaching motion profile, and by the target angle Degree, target angular velocity and target angular acceleration;
The communication management module, for being recorded to the target angle, target angular velocity and target angular acceleration.
In one embodiment, the top control module is communicatively coupled by first interface and algoritic module, algorithm mould Block is communicatively coupled by second interface and communication management module, and the first interface, second interface are to be based on creating on ROS The real-time input/output interface of OROCOS;
After the top control module receives the teaching movement instruction of control terminal transmission;Asynchronous triggering teaching Motor execution function is simultaneously Teaching motion planning function is called by first interface;
The algoritic module executes the teaching motion planning function, calculates the point on marginal position as exercise end; And pass through second interface for the target angle, target angular velocity and target angular acceleration;
The communication management module records the target angle, target angular velocity and target angular acceleration.
The technical solution of above-described embodiment, using the real-time input/output interface of OROCOS, top control module is logical by setting Letter agreement receives teaching movement instruction in a manner of asynchronous remote procedure call, and starting teaching movement instruction triggers function, calls The teaching Motor execution function of the robot motion planning of algoritic module calculates the motion profile of teaching according to the function, and real When the shapes such as each joint of the calculating machine arm target angle, target angular velocity and the target angular acceleration that are run on the motion profile State parameter, communication management module record above-mentioned state parameter, realize the teaching motion control process to robot;The skill Art scheme can carry out the reception of control instruction, parsing, and algorithm is called, executed, and parameter such as calculates and transmit in real time at the functions, can be with A complete robot teaching kinetic control system is constituted, robot control system development cost is reduced, improves control effect.
As embodiment, top control module executes algorithm flow, can be refering to what is shown in Fig. 3, Fig. 3 is that top control module executes algorithm Flow chart;It is specific as follows:
1) operator generates teaching movement instruction, the content of instruction, including starting point and movement by human-computer interaction interface Direction.
2) teaching movement instruction is sent in a manner of asynchronous remote procedure call from human-computer interaction interface by communication protocol To top control module.
It 3), can asynchronous triggering teaching Motor execution function after instruction reaches top control module.The function first determines whether controller Whether state machine is Ready state.If not Ready state, then refuse to execute this time instruction.
4) if controller state machine is Ready state, perform the following operations:
A) then pass through the teaching motion planning function of the Operational Caller method call algoritic module of OROCOS, And transmit order parameter.
B) controller state machine is converted into Active.PTP state.
As embodiment, algoritic module executes algorithm flow, can be refering to what is shown in Fig. 4, Fig. 4 is that algoritic module executes algorithm Flow chart;It is specific as follows:
1) teaching motion planning function is called by top control module, starts to execute teaching motion planning process.
2) check whether controller state machine is Active.Hands state, if not then exiting.
3) working region marginal position in this direction is calculated according to starting point and moving direction, and using the point as eventually Point.
If 4) arrived boundary or receive halt instruction:
A) it calls and stops S type velocity planning algorithm trajectory planning algorithm in real time, plan teaching motion profile;
B) target angle in each joint of one-time mechanical arm, target angular velocity and target angular acceleration are calculated every 1 millisecond;
C) by the target angle in each joint of mechanical arm, target angular velocity and target angular acceleration, it is sent to telecommunication management Module;
D) current movement instruction is recorded.
5) if it is the instruction of change direction:
A) it calls and stops S type velocity planning algorithm trajectory planning algorithm in real time, plan teaching motion profile;
B) target angle in each joint of one-time mechanical arm, target angular velocity and target angular acceleration are calculated every 1 millisecond;
C) by the target angle in each joint of mechanical arm, target angular velocity and target angular acceleration, it is sent to telecommunication management Module;
D) current movement instruction is recorded.
If e) stopped, using current point as starting point, working region in this direction is calculated according to moving direction Marginal position, and using the point as terminal;It repeats the above process.
If 6) inputted without new command, also without arriving boundary:
A) S type velocity planning algorithm trajectory planning algorithm is called, plans teaching motion profile;
B) target angle in each joint of one-time mechanical arm, target angular velocity and target angular acceleration are calculated every 1 millisecond;
C) by the target angle in each joint of mechanical arm, target angular velocity and target angular acceleration, it is sent to equipment communication Module;
D) current movement instruction is recorded.
The communication management module can be further used for the status information of read machine people's motor, be transported according to robot The status information of robot is fed back to master control mould by third interface by the status information of movable model calculating robot's mechanical arm The status information of robot is fed back to algoritic module by second interface by block;Wherein, the status information of the motor includes position It sets, speed and torque etc.;The status information of the mechanical arm includes joint angles, joint angular speed, joint angular acceleration, end Pose, end linear velocity, end angular speed, end linear acceleration and end linear acceleration etc..
It is as follows for S type and the real-time principle difference for stopping S type velocity planning algorithm:
1, S type velocity planning algorithm:
Fig. 5 is the stroke of classical S type speed planning, speed, acceleration, the derivative curve figure of acceleration;In S type speed In planning algorithm, generally have it is assumed hereinafter that:
jmin=-jmax amin=-amax vmin=-vmax
Min and max are expressed as minimum value and maximum value.
The time t of entire stroke can be divided into 3 stages in S type speed planning:
Boost phase: t ∈ [0, Ta], in this stage, acceleration can be since 0, with constant jmaxProportional linearity increases 0 is then lowered back to maximum value.
Constant velocity stage: t ∈ [Ta,Ta+Tv], this stage remains a constant speed.
Decelerating phase: t ∈ [Ta+Tv, T], wherein T=Ta+Tv+Td, this stage is the deceleration rank opposite with the acceleration stage Section, final acceleration return to 0.
Wherein we define:
Tj1: in boost phase, jerk keeps constant (jmax/jmin) period;
Tj2: in the decelerating phase, jerk keeps constant (jmax/jmin) period;
Ta: boost phase;
Tv: constant velocity stage;
Td: the decelerating phase;
T: total used time;
Boost phase peak acceleration;
Decelerating phase minimum acceleration;
vlim: maximum speed;
q0: priming stroke;
q1: terminal stroke;
v0: initial velocity;
v1: terminal velocity;
a0: initial acceleration;
a1: terminal acceleration;
It follows that the specific of S type speed planning is that mechanical arm is enabled to smoothly to accelerate to maximum operational speed, then Smoothly slow down again, and the overall used time is minimum.In we actually use, the speed of beginning and end can be generally enabled, is added Speed is all 0, i.e. v0=v1=0, v0=v1=0, hereafter we can use this as implied terms with simplified formula.
Before carrying out trajectory planning, need to calculate the parameter of curve, Fig. 6 is the parameter calculation process of S type speed planning Figure, is defined as algorithm .1.Wherein:
T=Ta+Td+Tv
t0=0, t1=Tj1,t2=Ta-Tj1,t3=Ta
t4=Ta+Tv,t5=T4+Tj2,t6=Tv-Tj2,t7=Tv (7)
With reference to Fig. 7, Fig. 7 is S type velocity planning algorithm whole flow process figure, is defined as algorithm .2, in which:
From the foregoing, it will be observed that because S type speed planning needs the coordinate of given starting point, and in tutorial function, terminal is not It can know in advance, so S type speed planning can not directly apply in the trajectory planning of tutorial function.
2, stop S type velocity planning algorithm in real time:
It is to stop S type velocity planning algorithm flow chart in real time with reference to Fig. 8, Fig. 8, is defined as algorithm .3;Utilize above-mentioned S type speed It spends planning algorithm and carries out mechanical arm trajectory planning, when running to t moment, when 0≤t≤T, need that mechanical arm is allowed to stop.This When need algorithm to cook up one section of suitable decelerating phase at once under current running status, enable mechanical arm can it is steady and It promptly stops, algorithm is as follows:
(1) if t ∈ [t0,t1);
(2) if t ∈ [t1,t2);
(3) if t ∈ [t2,t3);
q1=q1-vlimTv
t4=t3,t5=t5-Tv,t6=-Tv+t6,t7=-Tv+t7, T=t7,Tv=0 (11)
(4) if t ∈ [t3,t4);
It is above-mentioned to stop S type velocity planning algorithm detailed process to be real-time.
In order to be more clear the technical solution of the embodiment of the present invention, it is described below using hardware and software ring of the invention Border application example:
Refering to what is shown in Fig. 8, Fig. 8 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, which can be the PC machine or ARM of X86-based The development board of chip embedded framework, top control module access the control instruction of the human-computer interaction interface of control terminal.
Linux host can install following software: the real-time kernel of installation Xenomai or RTAI RT Preempt Patch;ROS, the softwares such as OROCOS, rFSM are installed.
Refering to what is shown in Fig. 9, Fig. 9 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.
1, for top control module:
(1) top control module is denoted as Ec_ using the Package of the orocreate-catkin-pkg method creation ROS of ROS Control_system, by inheriting the RTT::TaskContext class of OROCOS, is denoted as Ec_ then in Package control_system_component。
In the constructed fuction of Ec_control_system_component class, it is arranged to carry out following operation:
A) RTT::Input and the RTT::Output method of OROCOS, the input to module are utilized, output interface is determined Justice.
The interface wherein inputted includes:
1. the incoming diagnostic data of communication management module;
2. the incoming status 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 triggers, controller state machine is given in output.
B) function call interface is set using the Operational Caller method of OROCOS.
First, the call back function that setting event is reported: report that processing request responds to event, including generation error Timestamp, the information such as event level, and by event information, it is sent to human-computer interaction interface and shows.
Second, the call back function that setting alarm is arranged: according to diagnostic message, judge whether to generate alarm.For example, position, Whether speed, acceleration transfinite etc..
The control instruction triggering function of various motion plannings is arranged in third, these functions are by the corresponding sound to algoritic module Function is answered to be called.
C) it calls the Properties method of OROCOS to define the attribute of top control module, top control module is defined into a machinery The attribute of shoulder joint number.
(2) in StartHook () member function of Ec_control_system_component, setting executes 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 established by the communication protocol of ICE exploitation, communication protocol is called to provide Dynamic asynchronous remote procedure call (RPC) method, the readjustment letter that is responded of control instruction that human-computer interaction interface is initiated Number is bound.The call back function, according to ICE, (The Internet Communications Engine interconnects Netcom first Believe engine) first incoming parameter of the remote process asynchronous invoking method that provides, call type is judged, then according to this type The movement instruction of Selection and call corresponding sports planning triggers function.
(3) for CleanUpHook () member function of Ec_control_system_component, in order to enable should Function realizes automatic calling, may be arranged as performing the following operations when top control module terminates operation:
A) communication protocol interface for calling ICE exploitation, closes the communication connection with human-computer interaction interface.
(4) for controller state machine, refering to what is shown in Fig. 10, Figure 10 is the state change schematic diagram of controller state machine; Can be 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, point-to-point movement, linear motion, circle Arc movement, emergency stop state.Wherein, Active.Recovery, Active.Halt, Active.Hands, Active.ToZero, This eight states of Active.PTP, Active.Line, Active.Circle, Active.Stop form an Active state Set, the node transition rule of Active is effective to eight sub- states.For example, to any of eight states, write-in The state of controller state machine is transferred to Ready state (i.e. waiting instruction input shape from current state by " e_ready " event State).
Furthermore it is also possible to write the startup file of top control module using Lua language, the startup file be arranged to carry out as Lower movement:
A) pass through the import method of OROCOS, loading 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) the connect method for passing through OROCOS, by the input of top control module, output interface and algoritic module and communication tube The interface of reason module establishes connection.
E) the start method for passing through OROCOS runs top control module, and top control module will first call StartHook () function, Then default refreshing frequency is pressed, in real time periodically invoked UpdateHook () function.
2, for algoritic module:
Algoritic module is denoted as Ec_ using the Package of the orocreate-catkin-pkg method creation ROS of ROS Control_loop, by inheriting the RTT::TaskContext class of OROCOS, is denoted as Ec_ then in Package control_loop_component。
(1) in the constructed fuction of Ec_control_loop_component class, it is arranged to carry out following operation:
A) algoritic module utilizes RTT::Input and the RTT::Output method of OROCOS, and to input, output interface is carried out Definition.
The interface wherein inputted includes:
1. the 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 triggers, equipment state machine is given in output;
3. controller state machine event triggers, controller state machine is given in output.
B) function call interface is set using the Operational Caller method of OROCOS, the report of setting event connects Mouthful: the interface will trigger the return origin instruction response letter of the event report processing various motion plannings of function setup of top control module Number.
C) the Properties method of OROCOS is called to define the attribute of algoritic module, algoritic module defines a mechanical arm The attribute of joint number.
(2) it in StartHook () member function of Ec_control_loop_component, 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) it checks whether motor operation data channel has data, if no data directly exits, 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 class, which is set and is existed When algoritic module is run, according to frequency real time execution (being such as set as 100Hz) set by user, it can be set to execute following behaviour Make:
A) Read Controller state machine state;
B) according to the different conditions of controller state machine, different operation is executed:
I, it is moved if it is point-to-point, linear motion, circular motion, manual teaching, emergency stop returns to origin state.At this point, It performs the following operations:
If all instructions is sent jointly to telecommunication management mould less than 20 by the control instruction number of instruction buffer Block, and the state of controller state machine is changed into and waits instruction input state;
If the control instruction number of instruction buffer is greater than 20,20 of instruction fetch queue end, it is sent to logical Believe management module;
II, it if it is halted state, then does nothing.
(4) for Ec_control_loop_component class, point-to-point movement is defined, is moved along a straight line, circular motion, Manual teaching, emergency stop return to origin, pause, and the function call interfaces such as recovery are accomplished by
A) point-to-point moves, and linear motion, circular motion, manual teaching returns to the function of origin, and inside is accomplished by
It checks whether controller state machine is in and waits instruction input state.If not then exiting, and by relevant information Top control module processing is passed to by event reporting interface;
Read the current status information of motor;
According to the current state of motor, point-to-point movement, linear motion are called respectively, and circular motion, manual teaching returns to The motion planning of origin, and the motor control instruction of generation is saved in instruction buffer;
Controller state machine is set as corresponding state.For example point-to-point moves call back function, then by controller state machine It is set as point-to-point motion state.
B) suspend function, inside is accomplished by
Check whether controller state machine is point-to-point movement, is moved along a straight line, circular motion, manual teaching returns to origin Etc. states.If not then exiting, and relevant information is passed into top control module by event reporting interface and is 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 are accomplished by
Check whether controller state machine is halted state.Pass through event report if not then exiting, and by relevant information It accuses interface and passes to top control module processing;
The state of controller state machine is changed into the state before pause.
D) emergency stop function, inside are accomplished by
I, check whether controller state machine is point-to-point movement, is moved along a straight line, circular motion, manual teaching returns to original The states such as point.Top control module processing is passed to by event reporting interface if not then exiting, and by relevant information;
II, the current status information of motor is read;
III, motor control instruction buffer area is reset;
IV, speed planning motion planning is called, allows motor 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, is arranged to carry out following movement:
A) pass through the import method of OROCOS, loading algorithm module;
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 the connect method of OROCOS, by the input of algoritic module, output interface and top control module and communication tube The interface of reason module establishes connection.
E) the start method for passing through OROCOS, runs algoritic module, and algoritic module first calls StartHook () function, so The refreshing frequency of setting is pressed afterwards, in real time periodically invoked UpdateHook () function.
3, for communication management module:
Communication management module can pass through the ttyACM0 and Arm exploitation board communications in linux host minicom, Ke Yi A CANOpen master station protocol is run on the Arm development board, which can be set an instruction buffer area, at most may be used With 25 instructions of storage.
Communication management module can use RTT::Input and the RTT::Output method and robot algorithm mould of OROCOS Block and top control module are communicated.
Equipment state machine is established using rFSM software, the service logic of communication management module is controlled.
Communication management module is connect using RTT::Input and RTT::Output the method and apparatus state machine of OROCOS, The state of changeable equipment state machine, and reading state.
(1) communication module is created as the Package of ROS using the orocreate-catkin-pkg method of ROS, then exists In Package, by inheriting the RTT::TaskContext class of OROCOS, the Real time capable module of an OROCOS is created, Ec_ is denoted as component。
In the constructed fuction of Ec_component class, it is arranged to carry out following operation:
A) communication management module utilizes RTT::Input and the RTT::Output method of OROCOS, to input, output interface It is defined.
The interface wherein inputted includes:
1. the incoming control instruction data of algoritic module;
2. the state of equipment state machine;
The interface of output includes:
1. diagnostic data, output 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 trigger, equipment state machine is given in output.
B) the Operational Caller method defined function calling interface of OROCOS, communication management module definition are utilized The interface of event report triggers the event report processing function of top control module by the interface.
C) the Properties method of OROCOS is called to define the attribute of communication management module, communication management module defines one The attribute of a joint of mechanical arm number.
(2) in StartHook () member function of Ec_component, 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 driven initializes:
I, it is communicated by ttyACM0 with motor driver foundation;
II, motor command buffering queue empties;
III, motor is enabled, if enabling successfully, in next step, otherwise exit;
IV, 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, judges whether mechanical arm needs to be implemented and return to zero movement.If any joint angles of mechanical arm with Zero degree differs by more than 0.01 degree, then executes and return to zero movement, calls point-to-point motion planning, plans movement is returned to zero.
D) change the state of equipment state machine:
If the mechanical arm of robot, which needs to be implemented, returns to zero movement, controls equipment state machine and keep Init state constant; Otherwise, " e_nominal " event is sent to equipment state machine, the state of equipment state machine is converted into Active.Nominal.
(3) for UpdateHook () member function of Ec_component, which is set and is transported in communication management module When row, with frequency real time execution (being such as set as 1KHz) set by user, it 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 operation is executed:
I, if it is Init state, execute mechanical arm returns to zero movement.At this point, performing the following operations:
The clock of reading system, foundation return to zero Motion trajectory as a result, calculating the movement instruction of the moment motor, and will The single movement instruction is sent to CANOpen main website.
If movement has returned to zero success, " e_nominal " event is sent to equipment state machine, equipment state machine is converted to Active.Nominal, and the event is reported to top control module.
II, if it is Active.Nominal state.At this point, performing the following operations:
From the input channel of control instruction data, control instruction is read, and store into motor command buffering queue.
The existing number of instructions for reading CANOpen main website instruction buffer then disposably refers to from motor if it is less than 10 15 movement instructions of taking-up in buffering queue are enabled to be sent to CANOpen main website.If the number of instructions of motor command buffering queue It is less than 15, then disposable to be all sent to CANOpen main website.
III, if it is Active.Recovery state.At this point, communication management module is in recovery state.
At this point, restoring according to diagnostic message to system mode, if being successfully recovered, " e_ is sent to equipment state machine Equipment state machine is converted to Active.Nominal by nominal " event.And the event is reported to top control module.
If recovery is unsuccessful, " e_fault " event is sent to state machine, state machine is converted to Fault state, by the event It reports to top control module, and directly exits UpdateHook ().
IV, if it is Active.Halt state.At this point, module placed in a suspend state, performs the following operations: checking control Whether there is new instruction in the input channel of director data, if there is new command, reads control instruction, and store and arrive motor command In buffering queue.
V, if it is Active.Hands state.At this point, module is in manual mode, perform the following operations:
The clock of reading system, according to Motion trajectory as a result, calculate the movement instruction of the moment motor, and by the list Movement instruction is sent to CANOpen main website.
VI, it if it is Fault state, then directly exits UpdateHook ().
C) motor status is read, according to the normatron tool shoulder joint and end movement status information of mechanical arm, and By output data channel, algoritic module and top control module are passed to;
D) check that diagnostic message is then passed to calculation if there is error message by the control whether wrong report information of main website Method module and top control module." e_recovery " event is sent to equipment state machine, equipment state machine is converted to Active.Recovery state simultaneously reports the event to top control module.
(4) for CleanUpHook () member function of Ec_component, the function is when module terminates operation, certainly Dynamic to call, setting performs the following operations:
A) it is enabled to close motor driven;
B) motor driven connection is closed.
(5) with reference to shown in Figure 11, Figure 11 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 form the state set of an Active (enabled), and the node transition rule of Active is right Four sub- states are effective.
(6) Lua language is used, the startup file of the module is write, startup file setting executes following movement:
A) the import method for passing through OROCOS, loads communication management module;
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 the connect method of OROCOS, by the input of communication management module, output interface and top control module and calculation The interface of method mould etc. establishes connection.
E) the start method for passing through OROCOS, runs communication management module, and communication management module first calls StartHook () function, then good refreshing frequency by definition, in real time periodically invoked UpdateHook () function.
It for above-mentioned top control module, algoritic module and communication management module, is set as after operation, if in user's needs Way stops the module, while pressing the ctrl key and D key of keyboard.
To sum up embodiment guarantees the real-time of software program using the real time characteristic of OROCOS based on ROS and OROCOS Performance;The opening for making full use of ROS, top control module, algoritic module and communication management module based on ROS, OROCOS exploitation Real time communication is carried out, a complete robot controller software is collectively formed;It further established controller state machine and set Standby state machine, effectively manages the service logic of top control module, communication management module to realize.
By the RTT::Input of OROCOS, RTT::Output method establishes top control module, algoritic module and telecommunication management The data input of module, output channel are led to by the Operational Caller method defined function calling interface of OROCOS The Properties method for crossing OROCOS defines 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, for simplicity of description, not to above-mentioned reality It applies 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, all should be considered as described in this specification.
The embodiments described above only express several embodiments of the present invention, and the description thereof is more specific and detailed, but simultaneously It cannot therefore be construed as limiting the scope of the patent.It should be pointed out that coming for those of ordinary skill in the art It says, without departing from the inventive concept of the premise, various modifications and improvements can be made, these belong to protection of the invention Range.Therefore, the scope of protection of the patent of the invention shall be subject to the appended claims.

Claims (10)

1. a kind of robot teaching motion control method characterized by comprising
Receive the teaching movement instruction of control terminal transmission;Wherein, the teaching movement instruction includes starting point and the direction of motion;
Working region marginal position in this direction is calculated according to the starting point of mechanical arm tail end and moving direction, and by the side Point on edge position calls corresponding as exercise end according to the state of the teaching movement instruction and the exercise end Velocity planning algorithm plan teaching motion profile;
Target angle, the target angular velocity of each joint of calculating machine arm each position when being run on the teaching motion profile And target angular acceleration, and the target angle, target angular velocity and target angular acceleration are sent to equipment communication software and carried out Record;
Wherein, after receiving the teaching movement instruction, asynchronous triggering teaching Motor execution function simultaneously passes through first interface tune With teaching motion planning function;
The teaching motion planning function is executed, and plans teaching motion profile;
And the target angle, target angular velocity and target angular acceleration are sent to by equipment communication software by second interface It is recorded;
The first interface, second interface are based on the real-time input/output interface for creating OROCOS on ROS.
2. robot teaching motion control method according to claim 1, which is characterized in that if defeated currently without new command Enter, and mechanical arm does not move to boundary;S type velocity planning algorithm is called to plan teaching motion profile;
If receiving halt instruction or mechanical arm arrived boundary, calls and stop S type velocity planning algorithm in real time and plan teaching Motion profile.
3. robot teaching motion control method according to claim 2, which is characterized in that the calculating machine arm is each The step of the target angle of each position that joint is run on the teaching motion profile, target angular velocity and target angular acceleration Suddenly include:
According to the teaching motion profile, target angle, the mesh in each joint of one-time mechanical arm are calculated separately every setting time τ Mark angular speed and target angular acceleration.
4. robot teaching motion control method according to claim 2, which is characterized in that the S type speed planning is calculated The stroke of method includes three phases:
Boost phase: acceleration can increase linearly at a constant ratio maximum value since 0, be then lowered into 0;
Constant velocity stage: remain a constant speed movement;
Decelerating phase: being reduced speed now by the constant velocity stage, and final speed and acceleration return to 0.
5. robot teaching motion control method according to any one of claims 1 to 4, which is characterized in that the reception The step of teaching movement instruction of control terminal transmission further include: using preset communication protocol and with asynchronous remote procedure call Mode receives teaching movement instruction.
6. robot teaching motion control method according to claim 5, which is characterized in that refer to if receiving and changing direction It enables, calls and stop S type velocity planning algorithm planning teaching motion profile in real time;And after mechanical arm stopping, current point is made For starting point, working region marginal position in this direction is calculated according to moving direction, and using the marginal point as terminal, weight New calculating work space domain marginal position.
7. robot teaching motion control method according to claim 6, which is characterized in that the real-time stopping S type speed Spending planning algorithm process includes:
When mechanical arm runs to t moment, 0 < t < T cooks up one section of mechanical arm even running under current running status And the fastest decelerating phase;Wherein, T is teaching motion profile run duration.
8. robot teaching motion control method according to claim 7, which is characterized in that further include:
Before calling teaching motion planning function, the teaching Motor execution function judges whether controller state machine is to prepare shape State;
If so, teaching motion planning function described in the Operational Caller method call for passing through OROCOS, and will control Device state machine, which is switched to, executes teaching motion state;It is this time instructed if it is not, then refusing to execute.
9. robot teaching motion control method according to claim 8, which is characterized in that further include:
After calling teaching motion planning function, teaching motion planning process is executed according to the teaching motion planning function, and Check whether controller state machine is to execute teaching motion state;If so, the step of executing the motion profile for calculating teaching, no Then, execution process is exited.
10. a kind of robot teaching kinetic control system characterized by comprising top control module, algoritic module and telecommunication management Module;
The top control module, for receiving the teaching movement instruction of control terminal transmission;Wherein, the teaching movement instruction includes Initial point and the direction of motion;
The algoritic module, for calculating working region in this direction according to the starting point and moving direction of mechanical arm tail end Marginal position, and using the point on the marginal position as exercise end, according to the state of the teaching movement instruction and described Exercise end calls corresponding velocity planning algorithm planning teaching motion profile;The each joint of calculating machine arm is in the teaching Target angle, target angular velocity and the target angular acceleration of each position when being run on motion profile, and by the target angle, mesh Mark angular speed and target angular acceleration are sent to the communication management module;
The communication management module, for being recorded to the target angle, target angular velocity and target angular acceleration;
Wherein, after the top control module receives the teaching movement instruction that control terminal transmits, asynchronous triggering teaching Motor execution function And teaching motion planning function is called by first interface;
The algoritic module executes the teaching motion planning function, plans teaching motion profile, and passes through second interface for institute It states target angle, target angular velocity and target angular acceleration and is sent to communication management module;
The first interface, second interface are based on the real-time input/output interface for creating OROCOS on ROS.
CN201710256934.5A 2017-04-19 2017-04-19 Robot teaching motion control method and system Active CN106926243B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710256934.5A CN106926243B (en) 2017-04-19 2017-04-19 Robot teaching motion control method and system

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710256934.5A CN106926243B (en) 2017-04-19 2017-04-19 Robot teaching motion control method and system

Publications (2)

Publication Number Publication Date
CN106926243A CN106926243A (en) 2017-07-07
CN106926243B true CN106926243B (en) 2019-05-07

Family

ID=59437332

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710256934.5A Active CN106926243B (en) 2017-04-19 2017-04-19 Robot teaching motion control method and system

Country Status (1)

Country Link
CN (1) CN106926243B (en)

Families Citing this family (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107351087A (en) * 2017-08-29 2017-11-17 成都乐创自动化技术股份有限公司 A kind of crawl response control mehtod and system
CN109887396A (en) * 2017-12-06 2019-06-14 北京猎户星空科技有限公司 A kind of robot teaching system, method and device
CN108008729B (en) * 2017-12-13 2018-10-23 广州乾鼎电力科技发展有限公司 Robot infrared laser positioning motion trail planning method and device
CN109977708A (en) * 2017-12-28 2019-07-05 沈阳新松机器人自动化股份有限公司 A kind of 2 D code system and mobile platform teaching system for mobile platform teaching
CN108279674B (en) * 2018-01-18 2021-05-18 广州视源电子科技股份有限公司 Intelligent moving method, device, robot and storage medium
CN108237535A (en) * 2018-02-02 2018-07-03 广东工业大学 A kind of mechanical arm and its control system and method
CN108340374B (en) * 2018-02-08 2023-10-03 西北农林科技大学 Control system and control method for picking manipulator
CN110299062A (en) * 2018-03-21 2019-10-01 北京猎户星空科技有限公司 Mechanical arm teaching system, method, apparatus, medium, controller and mechanical arm
CN108608463B (en) * 2018-05-16 2021-07-30 珞石(山东)智能科技有限公司 Industrial robot conveyor belt dynamic tracking method
CN109726015B (en) * 2018-12-19 2021-05-04 福建天泉教育科技有限公司 Multi-end drawing board synchronization method based on state machine and terminal
CN113795795A (en) * 2019-06-26 2021-12-14 深圳市晟视科技有限公司 Method and system for realizing intelligent camera to control mobile platform
CN111730601B (en) * 2020-07-20 2021-01-01 季华实验室 Wearable demonstrator demonstration control method and device and electronic equipment
JP7512743B2 (en) * 2020-07-30 2024-07-09 セイコーエプソン株式会社 Teaching method and robot system
CN113125463B (en) * 2021-04-25 2023-03-10 济南大学 Teaching method and device for detecting weld defects of automobile hub
CN114102597B (en) * 2021-11-30 2023-05-16 伯朗特机器人股份有限公司 Mechanical arm articulation operation control method, electronic equipment and storage medium

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
AT509927B1 (en) * 2010-06-08 2015-05-15 Keba Ag METHOD FOR PROGRAMMING OR PRESENTING MOVEMENTS OR RUNS OF AN INDUSTRIAL ROBOT
CN102581849B (en) * 2012-01-05 2014-08-06 东南大学 Method for planning trajectories of industrial robot based on NC (numerical control) codes
CN103192397B (en) * 2012-01-09 2015-08-12 沈阳新松机器人自动化股份有限公司 Vision robot's off-line programing method and system
DE102014202145A1 (en) * 2014-02-06 2015-08-06 Kuka Roboter Gmbh A method of programming an industrial robot and associated industrial robots
CN105786483B (en) * 2016-01-05 2019-01-08 南京熊猫电子股份有限公司 Welding robot off-line programing system and its off-line programing method based on tablet computer

Also Published As

Publication number Publication date
CN106926243A (en) 2017-07-07

Similar Documents

Publication Publication Date Title
CN106926243B (en) Robot teaching motion control method and system
CN106950924B (en) Robot arc motion control method and system
CN106985140B (en) Robot point-to-point motion control method and system
CN106914900B (en) Robot control system based on ROS and OROCOS
US11184216B2 (en) State control method and apparatus
CN107030695B (en) Robot return origin point motion control method and system
CN106945044A (en) Robot pause motion control method and system
CN102195845B (en) Method, device and equipment for realizing active-standby switching of main control board
CN110457747A (en) A kind of network control system synergy emulation method based on NS3 and MATLAB
KR20070075957A (en) Robot control system for multi tasking based task
CN106060058A (en) Internet-based industrial robot remote control method
CN106959694A (en) robot linear motion control method and system
CN106945045B (en) communication method and system for robot control based on ROS and OROCOS
CN106956265B (en) Method and system for executing robot motion planning based on ROS and OROCOS
Bolla et al. From cloud-native to 5G-ready vertical applications: an industry 4.0 use case
CN116197919B (en) Robot control system and control method
CN106965181B (en) Robot control method and system based on ROS and OROCOS
CN115502975A (en) Robot scheduling method and device, electronic equipment and storage medium
CN102929159B (en) State control method and device for simulation model
CN113630280B (en) Distributed collaborative simulation operation platform
CN112737815B (en) Method and system for dynamically configuring event queue of network simulator
CN115705025A (en) Metallurgy automation control and edge calculation system and method, all-in-one machine and medium
Cicirelli et al. Actor-based simulation of PDEVS systems over HLA
KR20240083883A (en) Motion control method for industrial robot
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