WO2018192178A1 - 机器人点到点运动控制方法和系统 - Google Patents

机器人点到点运动控制方法和系统 Download PDF

Info

Publication number
WO2018192178A1
WO2018192178A1 PCT/CN2017/103436 CN2017103436W WO2018192178A1 WO 2018192178 A1 WO2018192178 A1 WO 2018192178A1 CN 2017103436 W CN2017103436 W CN 2017103436W WO 2018192178 A1 WO2018192178 A1 WO 2018192178A1
Authority
WO
WIPO (PCT)
Prior art keywords
point
motion
target
point motion
trajectory
Prior art date
Application number
PCT/CN2017/103436
Other languages
English (en)
French (fr)
Inventor
阳方平
Original Assignee
广州视源电子科技股份有限公司
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 广州视源电子科技股份有限公司 filed Critical 广州视源电子科技股份有限公司
Publication of WO2018192178A1 publication Critical patent/WO2018192178A1/zh

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor

Definitions

  • the present invention relates to the field of robot control technology, and in particular to a robot point-to-point motion control method and system.
  • ROS Robot Operating System
  • OROCOS Open Robot Control Software
  • robot point-to-point motion is an important mode of motion for robots.
  • OROCOS is adopted on ROS, but the existing technical scheme cannot constitute a complete robot controller software in the built architecture.
  • the characteristics of ROS and OROCOS cannot be fully utilized, and the system development cost is high and the control effect is poor.
  • a robot point-to-point motion control method includes:
  • the target angle, the target angular velocity, and the target angular acceleration of each position of each joint of the robot arm running on the point-to-point motion trajectory are calculated and forwarded to the control master station.
  • a robot point-to-point motion control system includes: a total control module, an algorithm module and a communication management module;
  • the master control module is configured to receive a point-to-point motion command transmitted by the control terminal;
  • the algorithm module is configured to parse the point-to-point motion instruction, obtain the time required for the target joint angle and the point-to-point motion; acquire the current joint angle of the mechanical arm, the current joint angular velocity, and the current joint angular acceleration; according to the point-to-point
  • the time required for the motion calculates the first trajectory through which the robot arm travels from the current joint angle to the target joint angle, and the calculated time between the current joint angular velocity and the current joint angular acceleration of the robot arm in the time required for the point-to-point motion, respectively.
  • a point-to-point motion trajectory is obtained according to the first trajectory, the second trajectory, and the third trajectory; and each position of each joint of the robot arm running on the point-to-point motion trajectory is calculated Target angle, target angular velocity, and target angular acceleration;
  • the communication management module is configured to forward a target angle, a target angular velocity, and a target angular acceleration of each joint of the robot arm on the point-to-point motion trajectory to the control primary station.
  • the above-mentioned robot point-to-point motion control method and system after receiving the point-to-point motion command of the control end, calculate the point-to-point motion trajectory, and calculate the target angle and target angular velocity of each joint of the robot arm on the motion trajectory in real time. And state parameters such as target angular acceleration, the above state parameters are forwarded to the control main station through the device communication software to realize the point-to-point motion control process of the robot; the technical solution can constitute a complete robot point-to-point motion control system, reducing The robot control system develops costs and improves control effects.
  • FIG. 1 is a flow chart of a robot point-to-point motion control method according to an embodiment of the present invention
  • FIG. 2 is a schematic structural view of a robot point-to-point motion control system
  • Figure 5 is an algorithm module execution algorithm state transition diagram
  • Figure 7 is a software architecture diagram based on ROS and OROCOS
  • Figure 8 is a schematic diagram showing the state change of the controller state machine
  • Figure 9 is a schematic diagram showing the state change of the device state machine.
  • the point-to-point motion refers to a motion process in which the joint angle of the mechanical arm moves from the current joint angle ⁇ 0 to the target joint angle ⁇ 1 .
  • the point-to-point motion instruction may be received in a manner of asynchronous remote procedure call using a preset communication protocol; for example, the point-to-point motion instruction includes a target joint angle ⁇ 1 and a time T required for point-to-point motion ;
  • the operator may generate a point-to-point motion instruction through the human-computer interaction interface, and the instruction does not need to pass parameters; the communication is developed through a preset communication protocol, such as the IEC (The Internet Communications Engine).
  • the protocol receives point-to-point motion instructions from the human-computer interface in the form of asynchronous remote procedure calls.
  • the point-to-point motion execution function is asynchronously triggered according to the point-to-point motion instruction and a point-to-point motion planning function is invoked through the first interface; wherein the first interface is based on real-time creation of OROCOS on the ROS Input/output interface.
  • the current joint angle, joint angular velocity, and joint angular acceleration of the robot arm are read, and according to the joint angle, joint angular velocity, and joint angular acceleration and point to
  • the point motion instruction calculates a point-to-point motion trajectory; wherein the joint angle, the joint angular velocity, and the joint angular acceleration include a joint angle, a joint angular velocity, and a joint angular acceleration;
  • the point-to-point motion execution function determines whether the controller state machine is in a Ready state; if so, the point-to-point motion planning function is invoked by an OROCOS Operational Caller method. And switching the controller state machine to perform a point-to-point motion (Active.PTP) state; if not, refusing to execute the instruction; wherein the controller state machine can be changed state and read state, It has the status of initialization, instruction wait, instruction execution, interrupt and enable.
  • the method for calculating a point-to-point motion trajectory may include the following:
  • the point-to-point motion command includes obtaining the target joint angle ⁇ 1 and the time T required for the point-to-point motion;
  • the point-to-point motion trajectory can be expressed as follows:
  • the target angle, the target angular velocity, and the target angular acceleration of each position of each joint of the robot arm running on the point-to-point motion trajectory may be calculated, and the target angle, the target angular velocity, and the target angular acceleration are transmitted to the device through the second interface.
  • the communication software is forwarded to the control master station; the second interface is based on a real-time input/output interface for creating an OROCOS on the ROS;
  • the step of calculating a target angle, a target angular velocity, and a target angular acceleration of each joint of each joint of the mechanical arm on the point-to-point motion trajectory may include the following:
  • calculation formula can be expressed as follows:
  • ⁇ t a 0 +a 1 t+a 2 t 2 +a 3 t 3 +a 4 t 4 +a 5 t 5
  • the calculation formula is called to calculate the target angle, the target angular velocity, and the target angular acceleration of each position of each joint of the robot arm running on the point-to-point motion trajectory.
  • the steps of calculating the target angle, the target angular velocity, and the target angular acceleration may include the following:
  • the time is counted from zero, and the target angle, the target angular velocity, and the target angular acceleration of each joint of the robot arm are continuously calculated according to the refresh rate; in the above embodiment, the ⁇ is generally 1 millisecond;
  • the transitional motion planning is started; if there is no new command input, the target angle of each joint of the mechanical arm is calculated once according to the calculation formula at the set time, the target Angular velocity and target angular acceleration, and sent to the device communication software;
  • the technical solution of the above embodiment can use the real-time input/output interface of OROCOS to receive the point-to-point motion instruction in an asynchronous remote procedure call by setting a communication protocol, start a point-to-point motion instruction trigger function, and invoke the robot motion planning.
  • the point-to-point motion execution function calculates the point-to-point motion trajectory according to the function, and calculates the state parameters such as the target angle, the target angular velocity and the target angular acceleration of the joints of the robot arm on the motion trajectory in real time, through the device communication software.
  • the above state parameter is forwarded to the control main station to realize the point-to-point motion control process of the robot; the technical solution can perform functions of receiving, parsing, algorithm calling, executing, parameter real-time calculation and transmission, etc., which can constitute a complete
  • the robot point-to-point motion control system reduces the development cost of the robot control system and improves the control effect.
  • the application of the controller state machine and the flag bit is combined to realize the optimal control of the algorithm processing process, and the control effect is further improved.
  • the present invention provides a robot point-to-point motion control system corresponding to the robot point-to-point motion control method.
  • FIG. 2 is a schematic structural diagram of a robot point-to-point motion control system, including: a total control module, an algorithm module, and a communication management module;
  • the master control module is configured to receive a point-to-point motion command transmitted by the control terminal;
  • the algorithm module is configured to parse the point-to-point motion instruction, obtain the time required for the target joint angle and the point-to-point motion; acquire the current joint angle of the mechanical arm, the current joint angular velocity, and the current joint angular acceleration; according to the point-to-point
  • the time required for the motion calculates the first trajectory through which the robot arm travels from the current joint angle to the target joint angle, and the calculated time between the current joint angular velocity and the current joint angular acceleration of the robot arm in the time required for the point-to-point motion, respectively.
  • a point-to-point motion trajectory is obtained according to the first trajectory, the second trajectory, and the third trajectory; and each position of each joint of the robot arm running on the point-to-point motion trajectory is calculated Target angle, target angular velocity, and target angular acceleration;
  • the communication management module is configured to forward a target angle, a target angular velocity, and a target angular acceleration of each joint of the robot arm on the point-to-point motion trajectory to the control primary station.
  • the total control module may be in communication connection with the algorithm module through the first interface, and the algorithm module is communicatively connected to the communication management module by using the second interface, where the first interface and the second interface are based on the ROS to create the OROCOS.
  • Real-time input/output interface
  • the master control module receives the point-to-point motion instruction by using a preset communication protocol and in an asynchronous remote procedure call; after receiving the point-to-point motion instruction, asynchronously triggers the point-to-point motion execution function and passes the first The interface calls the point-to-point motion planning function of the algorithm module;
  • the algorithm module After calling the point-to-point motion planning function, the algorithm module reads the current joint angle, joint angular velocity and joint angular acceleration of the mechanical arm, and calculates a point-to-point motion trajectory; and calculates each joint of the mechanical arm at the point a target angle, a target angular velocity, and a target angular acceleration of each position running on the point motion trajectory, and transmitting the target angle, the target angular velocity, and the target angular acceleration to the communication management module through the second interface;
  • the communication management module forwards the target angle, the target angular velocity, and the target angular acceleration to the control primary station.
  • the technical solution of the above embodiment utilizes the real-time input/output interface of the OROCOS, and the total control module receives the point-to-point motion instruction by setting the communication protocol in an asynchronous remote procedure call, starts the point-to-point motion instruction trigger function, and invokes the algorithm module.
  • the point-to-point motion execution function of the robot motion planning calculates the point-to-point motion trajectory according to the function, and calculates the state parameters such as the target angle, the target angular velocity and the target angular acceleration of the joints of the robot arm on the motion trajectory in real time
  • the communication management module forwards the above state parameters to the control main station to implement a point-to-point motion control process for the robot; the technical solution can perform functions of receiving, parsing, algorithm calling, executing, real-time parameter calculation and transmission of control commands, etc. It constitutes a complete robot point-to-point motion control system, which reduces the development cost of the robot control system and improves the control effect.
  • the total control module executes an algorithm flow, which can be referred to FIG. 3, and FIG. 3 is a total control module execution algorithm.
  • FIG. 3 is a total control module execution algorithm.
  • the operator generates a point-to-point motion command through the human-computer interaction interface, and the content of the instruction includes the target joint angle ⁇ 1 and the time T required for the motion.
  • the point-to-point motion command is sent from the human-computer interaction interface to the master control module by means of a communication protocol in an asynchronous remote procedure call.
  • the point-to-point motion execution function is triggered asynchronously. This function first determines if the controller state machine is in the Ready state. If it is not in the Ready state, the instruction is rejected.
  • the algorithm module executes an algorithm flow, which can be referred to FIG. 4, and FIG. 4 is a flowchart of an algorithm module execution algorithm; the details are as follows:
  • the point-to-point motion planning function is called by the master control module to start the point-to-point motion planning process.
  • the point-to-point motion trajectory is expressed as follows:
  • the joint angular position ⁇ t at any time t can be calculated according to the following formula, and the angular velocity Angular acceleration
  • ⁇ t a 0 +a 1 t+a 2 t 2 +a 3 t 3 +a 4 t 4 +a 5 t 5 (3)
  • FIG. 5 is an algorithm module performing an algorithm state transition diagram; setting a state flag of the algorithm module to a motion state.
  • the target angle, target angular velocity and target angular acceleration of each joint of the manipulator are calculated once every 1 millisecond according to formulas (3), (4), and (5).
  • the target angle, target angular velocity and target angular acceleration of each joint of the robot arm are sent to the communication management module.
  • the communication management module may be further configured to read state information of the robot motor, calculate state information of the robot arm according to the robot motion model, and feed back state information of the robot to the master control module through the third interface, and the state information of the robot Feedback to the algorithm module through the second interface; wherein the state information of the motor includes position, velocity, torque, etc.; the state information of the arm includes joint angle, joint angular velocity, joint angular acceleration, end pose, and end line speed , end angular velocity, end line acceleration, and end line acceleration.
  • FIG. 6 is a hardware structural model of a robot control system of an application example.
  • the software architecture of the robot controller is run on a Linux operating system, and the Linux host can be an X86-based PC or an ARM chip.
  • the development board of the embedded architecture, the control module of the master control module is connected to the human-computer interaction interface of the control terminal.
  • Linux hosts can be installed with the following software: install Xenomai or RTAI or RT Preempt real-time kernel patches; install ROS, OROCOS, rFSM and other software.
  • FIG. 7 is a software architecture diagram based on ROS and OROCOS; in the control process, the main control module, the algorithm module and the communication management module are run on the operating system.
  • the master control module uses ROS's orocreate-catkin-pkg method to create a ROS package, denoted as Ec_control_system, and then in the package, by inheriting ORTCOS's RTT::TaskContext class, denoted as Ec_control_system_component.
  • the input interface includes:
  • the status feedback information transmitted by the communication management module including the motor running status, etc.;
  • the output interfaces include:
  • the controller state machine event is triggered and output to the controller state machine.
  • a callback function for setting an event report responding to the event report processing request, including generating an error timestamp, an event level, and the like, and transmitting the event information to the human-machine interaction interface for display.
  • control command trigger functions for various motion plans. These functions will call the corresponding response functions of the algorithm module.
  • FIG. 8 is a schematic diagram of the state change of the controller state machine; Init, Ready, Fault, Active.Recovery, Active.Halt, Active.Hands, Active.ToZero can be set.
  • Active.PTP, Active.Line, Active.Circle, Active.Stop have a total of eleven states, representing initialization, waiting for command input, recovery, pause, manual teaching, return to origin, point-to-point motion, linear motion, Arc motion, emergency stop state.
  • the eight states of Active.Recovery, Active.Halt, Active.Hands, Active.ToZero, Active.PTP, Active.Line, Active.Circle, and Active.Stop form a collection of Active states, Active state transition rules, Valid for all eight substates. For example, for any of the eight states, an "e_ready" event is written to transition the state of the controller state machine from the current state to the Ready state (ie, waiting for the instruction input state).
  • the master control module will first call the StartHook () function, and then according to the preset refresh frequency, the UpdateHook () function is called periodically in real time.
  • the algorithm module uses ROS's orocreate-catkin-pkg method to create a ROS package, denoted as Ec_control_loop, and then in the Package, by inheriting ORTCOS's RTT::TaskContext class, denoted as Ec_control_loop_component.
  • the algorithm module uses the ROT::Input and RTT::Output methods of OROCOS to define the input and output interfaces.
  • the input interface includes:
  • the output interfaces include:
  • Controller state machine event trigger output to the controller state machine.
  • the algorithm module defines the properties of a number of mechanical arm joints.
  • the 20 at the end of the instruction queue are fetched and sent to the communication management module;
  • Ec_control_loop_component For the Ec_control_loop_component class, define point-to-point motion, linear motion, arc motion, manual teaching, emergency stop, return to origin, pause, recovery and other function call interfaces, as follows:
  • controller state machine Set the controller state machine to the appropriate state.
  • the point-to-point motion callback function sets the controller state machine to point-to-point motion.
  • controller state machine Check whether the controller state machine is point-to-point motion, linear motion, circular motion, manual teaching, return to the origin and other states. If not, exit and pass the relevant information to the master control module through the event reporting interface;
  • d) define the refresh rate of the algorithm module, the priority level of the thread
  • the algorithm module runs the algorithm module through the start method of OROCOS.
  • the algorithm module first calls the StartHook() function, and then periodically calls the UpdateHook() function in real time according to the set refresh frequency.
  • the communication management module can communicate with the Arm development board through ttyACM0 in the Linux host minicom.
  • a CANOpen master protocol can be run on the Arm development board.
  • the master station protocol can set an instruction buffer area and store up to 25 instructions.
  • the communication management module can communicate with the robot algorithm module and the master control module by using the ROT::Input and RTT::Output methods of OROCOS.
  • the device state machine is established by using the rFSM software to control the business logic of the communication management module.
  • the communication management module uses ORTCOS' RTT::Input and RTT::Output methods to connect to the device state machine to change the state of the device state machine and read the state.
  • the communication module is created as a package of ROS using the orocreate-catkin-pkg method of ROS, and then in the package, by inheriting the RTT::TaskContext class of OROCOS, an real-time module of OROCOS is created, which is recorded as Ec_component.
  • the communication management module uses the ROT::Input and RTT::Output methods of OROCOS to define the input and output interfaces.
  • the input interface includes:
  • the output interfaces include:
  • the function call interface is defined by the OROCOS Operational Caller method, and the communication management module defines an event report interface through which the event report processing function of the master control module is triggered.
  • the communication management module defines the properties of the number of joints of the robot arm.
  • the position of the motor it is judged whether the arm needs to perform the zero return movement. If the angle of any joint of the robot arm differs from the zero degree by more than 0.01 degrees, the zero return motion is performed, and the point-to-point motion planning is called to plan the zero return motion.
  • the control device state machine remains in the Init state; otherwise, an "e_nominal" event is sent to the device state machine to convert the state of the device state machine to Active. Nominal.
  • the clock of the system is read, the motion prediction result of the motor is calculated according to the result of the zero return motion trajectory, and the single motion instruction is sent to the CANOpen master station.
  • the "e_nominal" event is sent to the device state machine, the device state machine is converted to Active.Nominal, and the event is reported to the master control module.
  • control command is read and stored in the motor command buffer queue.
  • the system state is restored. If the recovery is successful, an "e_nominal" event is sent to the device state machine, and the device state machine is converted to Active. Nominal. Report the event to the master control module.
  • the module if it is Active.Halt state. At this time, the module is in the pause state, and performs the following operations: checking whether there is a new instruction in the input channel of the control instruction data, and if there is a new instruction, reading the control instruction and storing it in the motor instruction buffer queue.
  • V if it is the Active.Hands state. At this point, the module is in manual mode and performs the following operations:
  • the clock of the system is read, the motion instruction of the motor at the moment is calculated according to the motion trajectory planning result, and the single motion instruction is sent to the CANOpen master station.
  • FIG. 9 is a schematic diagram showing a state change of the device state machine.
  • the Active.Recovery, Active.Hands, Active.Halt, and Active.Nominal states form an Active state set, and the Active state transition rule is valid for all four child states.
  • the input and output interfaces of the communication management module are connected with the interfaces of the master control module and the algorithm module.
  • the communication management module first calls the StartHook() function, and then periodically calls the UpdateHook() function in real time according to the defined refresh frequency.
  • master control module For the above-mentioned master control module, algorithm module and communication management module, it is set to be, after running, if the user needs to stop the module midway, press the ctrl key and the D key of the keyboard at the same time.
  • the real-time performance of OROCOS is used to ensure the real-time performance of the software program; the openness of ROS is fully utilized, and the total control module, algorithm module and communication management module developed by ROS and OROCOS are used for real-time communication. Together, they form a complete robot controller software; further establish the controller state machine and device state machine to realize the effective management of the business logic of the master control module and the communication management module.
  • the RTO::Input, RTT::Output methods of OROCOS are used to establish the data input and output channels of the master control module, the algorithm module and the communication management module.
  • the function call interface is defined by the OROCOS Operational Caller method, and the total control is defined by the OROCOS Properties method. Attributes of modules, algorithm modules, and communication management modules. Therefore, the independence and decoupling between the master control module, the algorithm module and the communication management module are guaranteed.

Abstract

一种机器人点到点运动控制方法和系统，其中方法包括：接收控制端传送的点到点运动指令；解析获取目标关节角度和点到点运动需要的时间；根据点到点运动需要的时间计算机械臂由当前关节角度运行到目标关节角度经过的第一轨迹，以及分别计算机械臂在点到点运动需要的时间内由当前关节角速度和当前关节加速度降为0所经过的第二轨迹和第三轨迹；根据第一轨迹、第二轨迹和第三轨迹得到点到点的运动轨迹；计算机械臂各个关节在点到点运动轨迹上运行的各个位置的目标角度、目标角速度和目标角加速度转发至控制主站。该点到点运动控制方法可以构成一个完整的机器人点到点运动控制系统，降低机器人控制系统开发成本，提高控制效果。

Description

机器人点到点运动控制方法和系统 技术领域
本发明涉及机器人控制技术领域，特别是涉及一种机器人点到点运动控制方法和系统。
背景技术
Robot Operating System(ROS)是开源的机器人操作系统，可以为机器人开发者提供一个标准化的、开源的编程框架。但是ROS目前不支持实时线程操作。Open Robot Control Software(OROCOS)也是一种开源的机器人控制软件编程框架，它的特点是支持实时的线程操作，但是它的开放性，通用性没有ROS好。
因此，机器人点到点运动是机器人一种重要运动方式，目前在一些应用方案中，在ROS上采用OROCOS，但现有技术方案在搭建的架构上，不能构成一个完整的机器人控制器软件，在执行点到点运动时，无法充分利用ROS和OROCOS的特性，系统开发成本高，控制效果差。
发明内容
基于此，有必要针对上述系统开发成本高技术问题，提供一种机器人点到点运动控制方法，降低系统开发成本，提高控制效果。
一种机器人点到点运动控制方法，包括：
接收控制端传送的点到点运动指令；对点到点运动指令进行解析，获取目标关节角度和点到点运动需要的时间；
获取机械臂的当前关节角度、当前关节角速度和当前关节角加速度；根据点到点运动需要的时间计算机械臂由当前关节角度运行到目标关节角度经过的第一轨迹，以及分别计算机械臂在所述点到点运动需要的时间内由当前关节角速度和当前关节角加速度降为0所经过的第二轨迹和第三轨迹；根据第一轨迹、第二轨迹和第三轨迹得到点到点的运动轨迹；
计算机械臂各个关节在所述点到点运动轨迹上运行的各个位置的目标角度、目标角速度和目标角加速度转发至控制主站。
一种机器人点到点运动控制系统，包括：总控模块、算法模块和通信管理模块；
所述总控模块，用于接收控制端传送的点到点运动指令；
所述算法模块，用于对点到点运动指令进行解析，获取目标关节角度和点到点运动需要的时间；获取机械臂的当前关节角度、当前关节角速度和当前关节角加速度；根据点到点运动需要的时间计算机械臂由当前关节角度运行到目标关节角度经过的第一轨迹，以及分别计算机械臂在所述点到点运动需要的时间内由当前关节角速度和当前关节角加速度降为0所经过的第二轨迹和第三轨迹；根据第一轨迹、第二轨迹和第三轨迹得到点到点的运动轨迹；以及计算机械臂各个关节在所述点到点运动轨迹上运行的各个位置的目标角度、目标角速度和目标角加速度；
所述通信管理模块，用于将机械臂各个关节在所述点到点运动轨迹上运行的各个位置的目标角度、目标角速度和目标角加速度转发至控制主站。
上述机器人点到点运动控制方法和系统，在接收到控制端的点到点运动指令后，计算点到点的运动轨迹，并实时计算机械臂各个关节在该运动轨迹上运行的目标角度、目标角速度和目标角加速度等状态参数，通过设备通信软件将上述状态参数转发至控制主站，实现对机器人的点到点运动控制过程；该技术方案可以构成一个完整的机器人点到点运动控制系统，降低机器人控制系统开发成本，提高控制效果。
附图说明
图1是本发明实施例的机器人点到点运动控制方法流程图；
图2是机器人点到点运动控制系统结构示意图；
图3是总控模块执行算法流程图；
图4是算法模块执行算法流程图；
图5是算法模块执行算法状态转换图；
图6是一应用实例的机器人控制系统的硬件结构模型；
图7是基于ROS和OROCOS的搭建的软件架构图；
图8是控制器状态机的状态变化示意图；
图9是设备状态机的状态变化示意图。
具体实施方式
下面结合附图阐述本发明的机器人点到点运动控制方法的实施例。
本发明实施例中，所述点到点运动，是指机械臂的关节角从当前关节角度θ0运动到目 标关节角度θ1的运动过程。
S10，接收控制端传送的点到点运动指令；对点到点运动指令进行解析，获取目标关节角度和点到点运动需要的时间；
上述步骤中，可以利用预设的通信协议并以异步远程过程调用的方式接收点到点运动指令；例如，所述点到点运动指令包括目标关节角度θ1和点到点运动需要的时间T；
在此过程中，可以是操作者通过人机交互界面生成点到点运动指令，该指令无需传递参数；通过预设的通信协议，如基于IEC(The Internet Communications Engine，互联网通信引擎)开发的通信协议，以异步远程过程调用的方式从人机交互界面接收点到点运动指令。
在一个实施例中，根据所述点到点运动指令异步触发点到点运动执行函数并通过第一接口调用点到点运动规划函数；其中，所述第一接口是基于ROS上创建OROCOS的实时输入/输出接口。
S20，获取机械臂的当前关节角度、当前关节角速度和当前关节角加速度；根据点到点运动需要的时间计算机械臂由当前关节角度运行到目标关节角度经过的第一轨迹，以及分别计算机械臂在所述点到点运动需要的时间内由当前关节角速度和当前关节角加速度降为0所经过的第二轨迹和第三轨迹；根据第一轨迹、第二轨迹和第三轨迹得到点到点的运动轨迹；
在一个实施例中，在调用所述点到点运动规划函数后，读取机械臂当前的关节角度、关节角速度和关节角加速度，并根据所述关节角度、关节角速度和关节角加速度和点到点运动指令计算点到点的运动轨迹；其中，所述关节角度、关节角速度和关节角加速度包括关节角度、关节角速度和关节角加速度；
进一步地，调用点到点运动规划函数前，所述点到点运动执行函数判断控制器状态机是否为准备(Ready)状态；若是，通过OROCOS的Operational Caller方法调用所述点到点运动规划函数，并将控制器状态机切换为执行点到点运动(Active.PTP)状态；若否，则拒绝执行此次指令；其中，所述控制器状态机的可被改变状态、并读取状态，设有初始化、指令等待、指令执行、中断和使能对应的状态。
进一步地，在调用点到点运动规划函数后，根据所述点到点运动规划函数执行点到点运动规划流程，并检查控制器状态机是否为执行点到点运动状态；若是，执行所述计算点到点的运动轨迹的步骤，否则，退出执行流程。
作为实施例，所述计算点到点的运动轨迹的方法，可以包括如下：
(1)获取机械臂当前的关节角度θ0、关节角速度
Figure PCTCN2017103436-appb-000001
和关节角加速度
Figure PCTCN2017103436-appb-000002
(2)从点到点运动指令包括获取目标关节角度θ1和点到点运动需要的时间T；
(3)根据所述关节角度θ0、关节角速度
Figure PCTCN2017103436-appb-000003
和关节角加速度
Figure PCTCN2017103436-appb-000004
以及目标关节角度θ1，将机械臂关节目标角速度和目标角加速度设为0，确定点到点的运动轨迹；
作为实施例，所述点到点的运动轨迹可以表示如下：
S(t)＝a0+a1t+a2t2+a3t3+a4t4+a5t5
a0＝θ0
Figure PCTCN2017103436-appb-000005
Figure PCTCN2017103436-appb-000006
Figure PCTCN2017103436-appb-000007
Figure PCTCN2017103436-appb-000008
Figure PCTCN2017103436-appb-000009
式中，t是运行时间；ai，i＝1,…,5是系数。
S30，计算机械臂各个关节在所述点到点运动轨迹上运行的各个位置的目标角度、目标角速度和目标角加速度转发至控制主站。
具体的，可以计算机械臂各个关节在所述点到点运动轨迹上运行的各个位置的目标角度、目标角速度和目标角加速度，通过第二接口将目标角度、目标角速度和目标角加速度发送给设备通信软件转发至控制主站；所述第二接口是基于ROS上创建OROCOS的实时输入/输出接口；
在一个实施例中，所述计算机械臂各个关节在所述点到点运动轨迹上运行的各个位置的目标角度、目标角速度和目标角加速度的步骤，可以包括如下：
(1)根据所述点到点的运动轨迹计算任意时刻t时的关节角位置、角速度
Figure PCTCN2017103436-appb-000010
和角加速度
Figure PCTCN2017103436-appb-000011
进一步地，所述计算公式可以表示如下：
θt＝a0+a1t+a2t2+a3t3+a4t4+a5t5
Figure PCTCN2017103436-appb-000012
Figure PCTCN2017103436-appb-000013
式中，任意时刻t时的关节角位置θt、角速度
Figure PCTCN2017103436-appb-000014
和角加速度
Figure PCTCN2017103436-appb-000015
(2)根据点到点运动需要的时间T，调用所述计算公式计算机械臂各个关节在所述点到点运动轨迹上运行的各个位置的目标角度、目标角速度和目标角加速度。
进一步地，计算目标角度、目标角速度和目标角加速度的步骤可以包括如下：
(a)在机械臂开始运动后，由0开始每隔设定时间τ统计机械臂的运动时间tn；τ＝1/f，f表示刷新频率；
即由零开始统计时间，按照刷新率不断计算机械臂各个关节的目标角度、目标角速度和目标角加速度；上述实施例中，一般情况下，所述τ为1毫秒；
(b)若所述运动时间满足：tn≤aT，每隔设定时间τ依据所述计算公式分别计算一次机械臂每个关节的目标角度、目标角速度和目标角加速度；一般情况下，所述a＝70％；
(c)若运动时间tn满足：aT<tn<T，将标志位更改为过渡状态，将控制器状态机状态设定为准备状态；
进一步地，在过渡状态下，若有新指令输入，则启动过渡运动规划；若没有新指令输入，则每隔设定时间依据所述计算公式分别计算一次机械臂每个关节的目标角度，目标角速度和目标角加速度，并发送给设备通信软件；
(d)若运动时间tn满足：tn≥T，点到点运动结束，将标志位更改为等待状态。
上述实施例的技术方案，可以利用OROCOS的实时输入/输出接口，通过设定通信协议以异步远程过程调用的方式接收点到点运动指令，启动点到点运动指令触发函数，调用机器人运动规划的点到点运动执行函数，根据该函数计算点到点的运动轨迹，并实时计算机械臂各个关节在该运动轨迹上运行的目标角度、目标角速度和目标角加速度等状态参数，通过设备通信软件将上述状态参数转发至控制主站，实现对机器人的点到点运动控制过程；该技术方案可以进行控制指令的接收、解析，算法调用、执行，参数实时计算和传输等功能，可以构成一个完整的机器人点到点运动控制系统，降低机器人控制系统开发成本，提高控制效果。另外结合了控制器状态机和标志位的应用，实现对算法处理过程的优化控制，进一步提高了控制效果。
针对于机器人点到点运动控制方法，本发明提供了该控制方法对应的机器人点到点运动控制系统，
参考图2所示，图2是机器人点到点运动控制系统结构示意图，包括：总控模块、算法模块和通信管理模块；
所述总控模块，用于接收控制端传送的点到点运动指令；
所述算法模块，用于对点到点运动指令进行解析，获取目标关节角度和点到点运动需要的时间；获取机械臂的当前关节角度、当前关节角速度和当前关节角加速度；根据点到点运动需要的时间计算机械臂由当前关节角度运行到目标关节角度经过的第一轨迹，以及分别计算机械臂在所述点到点运动需要的时间内由当前关节角速度和当前关节角加速度降为0所经过的第二轨迹和第三轨迹；根据第一轨迹、第二轨迹和第三轨迹得到点到点的运动轨迹；以及计算机械臂各个关节在所述点到点运动轨迹上运行的各个位置的目标角度、目标角速度和目标角加速度；
所述通信管理模块，用于将机械臂各个关节在所述点到点运动轨迹上运行的各个位置的目标角度、目标角速度和目标角加速度转发至控制主站。
作为实施例，所述总控模块可以通过第一接口与算法模块进行通信连接，算法模块通过第二接口与通信管理模块进行通信连接，所述第一接口、第二接口是基于ROS上创建OROCOS的实时输入/输出接口；
所述总控模块利用预设的通信协议并以异步远程过程调用的方式接收点到点运动指令；在接收到所述点到点运动指令后，异步触发点到点运动执行函数并通过第一接口调用算法模块的点到点运动规划函数；
所述算法模块在调用所述点到点运动规划函数后，读取机械臂当前的关节角度、关节角速度和关节角加速度，并计算点到点的运动轨迹；计算机械臂各个关节在所述点到点运动轨迹上运行的各个位置的目标角度、目标角速度和目标角加速度，通过第二接口将所述目标角度、目标角速度和目标角加速度发送给通信管理模块；
所述通信管理模块将所述目标角度、目标角速度和目标角加速度转发至控制主站。
上述实施例的技术方案，利用OROCOS的实时输入/输出接口，总控模块通过设定通信协议以异步远程过程调用的方式接收点到点运动指令，启动点到点运动指令触发函数，调用算法模块的机器人运动规划的点到点运动执行函数，根据该函数计算点到点的运动轨迹，并实时计算机械臂各个关节在该运动轨迹上运行的目标角度、目标角速度和目标角加速度等状态参数，通信管理模块将上述状态参数转发至控制主站，实现对机器人的点到点运动控制过程；该技术方案可以进行控制指令的接收、解析，算法调用、执行，参数实时计算和传输等功能，可以构成一个完整的机器人点到点运动控制系统，降低机器人控制系统开发成本，提高控制效果。
作为实施例，总控模块执行算法流程，可以参考图3所示，图3是总控模块执行算法 流程图；具体如下：
1)操作者通过人机交互界面生成点到点运动指令，指令的内容，包括目标关节角度θ1，运动需要的时间T。
2)点到点运动指令通过通信协议，以异步远程过程调用的方式从人机交互界面发送给总控模块。
3)指令到达总控模块后，会异步触发点到点运动执行函数。该函数首先判断控制器状态机是否为Ready状态。若不是Ready状态，则拒绝执行此次指令。
4)若控制器状态机是Ready状态，则执行如下操作：
a)则通过OROCOS的Operational Caller方法调用算法模块的点到点运动规划函数，并且传递指令参数。
b)将控制器状态机转换为Active.PTP状态。
5)判断控制器状态机是否重新变为Ready状态。若是Ready状态，则本次运动完成。
作为实施例，算法模块执行算法流程，可以参考图4所示，图4是算法模块执行算法流程图；具体如下：
1)点到点运动规划函数被总控模块调用，开始执行点到点运动规划流程。
2)检查控制器状态机是否为Active.PTP状态，若不是则退出。
3)读取机械臂当前的关节角度θ0，关节角速度
Figure PCTCN2017103436-appb-000016
关节角加速度
Figure PCTCN2017103436-appb-000017
采用五次多项式方法，生成点到点运动轨迹。
该方法的原理如下：
点到点的运动轨迹表示如下：
S(t)＝a0+a1t+a2t2+a3t3+a4t4+a5t5         (1)
式中，t是运行时间；ai,i＝1,…,5是系数。
由于已知机械臂当前的关节角度θ0，关节角速度
Figure PCTCN2017103436-appb-000018
关节角加速度
Figure PCTCN2017103436-appb-000019
以及目标关节角度θ1。并将目标的关节角速度和角加速度设为0。则可求得系数：
a0＝θ0
Figure PCTCN2017103436-appb-000020
Figure PCTCN2017103436-appb-000021
Figure PCTCN2017103436-appb-000022
Figure PCTCN2017103436-appb-000023
Figure PCTCN2017103436-appb-000024
然后就可以根据以下公式计算出任意时刻t时的关节角位置θt，角速度
Figure PCTCN2017103436-appb-000025
角加速度
Figure PCTCN2017103436-appb-000026
θt＝a0+a1t+a2t2+a3t3+a4t4+a5t5        (3)
Figure PCTCN2017103436-appb-000027
Figure PCTCN2017103436-appb-000028
4)将运动时间tn记为0。参考图5所示，图5是算法模块执行算法状态转换图；将算法模块的状态标志位设为运动状态。
5)算法模块的UpdateHook()函数检测到状态标志位为运动状态后，且运动时间tn不超过运动时长T的70％：
a)每隔1毫秒依据公式(3)、(4)、(5)分别计算一次机械臂每个关节的目标角度，目标角速度和目标角加速度。
b)将机械臂每个关节的目标角度，目标角速度和目标角加速度，发送给通信管理模块。
6)若运动时间tn满足：0.7T<tn<T:
a)将算法模块的标志位更改为过渡状态。
b)将控制器状态机状态设定为Ready状态。
c)若有新指令输入，则启动过渡运动规划。
d)若没有新指令输入，则每隔1毫秒依据公式(3)、(4)、(5)分别计算一次机械臂每个关节的目标角度，目标角速度和目标角加速度。并将机械臂每个关节的目标角度，目标角速度和目标角加速度，发送给通信管理模块。
7)若运动时间tn满足：tn≥T:
a)本次点到点运动结束。
b)将算法模块的标志位更改为等待状态。
所述通信管理模块，可以进一步用于读取机器人电机的状态信息，根据机器人运动模型计算机器人机械臂的状态信息，将机器人的状态信息通过第三接口反馈给总控模块，将机器人的状态信息通过第二接口反馈给算法模块；其中，所述电机的状态信息包括位置、速度和力矩等；所述机械臂的状态信息包括关节角度、关节角速度、关节角加速度、末端位姿、末端线速度、末端角速度、末端线加速度和末端线加速度等。
为了更加清晰本发明的实施例的技术方案，下面阐述采用本发明的硬件和软件环境应用实例：
参考图6所示，图6是一应用实例的机器人控制系统的硬件结构模型，在机器人控制器的搭建软件架构，运行于Linux操作系统，该Linux主机可以是X86架构的PC机，或者ARM芯片嵌入式架构的开发板，总控模块接入控制端的人机交互界面的控制指令。
Linux主机可以安装如下软件：安装Xenomai或者RTAI或者RT Preempt的实时内核补丁；安装ROS，OROCOS，rFSM等软件。
参考图7所示，图7是基于ROS和OROCOS的搭建的软件架构图；在控制过程中，操作系统上运行总控模块、算法模块和通信管理模块。
1、对于总控模块：
(1)总控模块使用ROS的orocreate-catkin-pkg方法创建ROS的Package，记为Ec_control_system，然后在Package中，通过继承OROCOS的RTT：：TaskContext类，记为Ec_control_system_component。
在Ec_control_system_component类的构造函数中，设置为执行如下操作：
a)利用OROCOS的RTT：：Input与RTT：：Output方法，对模块的输入，输出接口进行定义。
其中输入的接口包括：
①通信管理模块传入的诊断数据；
②通信管理模块传入的状态反馈信息：包括电机运行状态等等；
③控制器状态机的状态；
输出的接口包括：
①控制器状态机事件触发，输出给控制器状态机。
b)利用OROCOS的Operational Caller方法设置函数调用接口。
第一，设置事件报告的回调函数：对事件报告处理请求进行响应，包括生成错误的时间戳，事件级别等信息，并将事件信息，发送给人机交互界面显示。
第二，设置警报设置的回调函数：依据诊断信息，判断是否生成警报。比如，位置，速度，加速度是否超限等等。
第三，设置各种运动规划的控制指令触发函数，这些函数将对算法模块的相应响应函数进行调用。
c)调用OROCOS的Properties方法定义总控模块的属性，将总控模块定义一个机械臂关节个数的属性。
(2)在Ec_control_system_component的StartHook()成员函数中，设置执行如下操作：
a)检查日志报告是否正常，若异常直接退出，并将相关信息通过事件报告接口传递给总控模块处理；
b)通过ICE开发的通信协议建立与人机交互界面的通信连接，调用通信协议提供的动态异步远程过程调用(RPC)方法，对人机交互界面发起的控制指令进行响应的回调函数进行绑定。该回调函数，首先根据ICE(The Internet Communications Engine，互联网通信引擎)提供的远程过程异步调用方法传入的第一个参数，判断调用类型，然后依据此类型选择调用相应运动规划的运动指令触发函数。
(3)对于Ec_control_system_component的CleanUpHook()成员函数，为了使得该函数在总控模块结束运行时，实现自动调用，还可以设置为执行如下操作：
a)调用ICE开发的通信协议接口，关闭与人机交互界面的通信连接。
(4)对于控制器状态机，参考图8所示，图8是控制器状态机的状态变化示意图；可以设置Init、Ready、Fault、Active.Recovery、Active.Halt、Active.Hands、Active.ToZero、Active.PTP、Active.Line、Active.Circle、Active.Stop共十一个状态，分别代表初始化、等待指令输入、恢复、暂停、手动示教、回到原点、点到点运动、直线运动、圆弧运动、急停状态。其中，Active.Recovery、Active.Halt、Active.Hands、Active.ToZero、Active.PTP、Active.Line、Active.Circle、Active.Stop这八个状态组成一个Active状态的集合，Active的状态转移规则，对八个子状态均有效。例如，给八个状态中的任一个，写入“e_ready”事件，将控制器状态机的状态从当前状态转移到Ready状态(即等待指令输入状态)。
另外，还可以使用Lua语言，编写总控模块的启动文件，该启动文件设置为执行如下动作：
a)通过OROCOS的import方法，加载模块进行运行；
b)定义模块的刷新频率，线程的优先级别；
c)对模块的属性进行赋值；
d)通过OROCOS的connect方法，将总控模块的输入、输出接口和算法模块和通信管理模块的接口建立连接。
e)通过OROCOS的start方法，运行总控模块，总控模块将先调用StartHook()函数，然后按预设刷新频率，实时地周期性调用UpdateHook()函数。
2、对于算法模块：
算法模块使用ROS的orocreate-catkin-pkg方法创建ROS的Package，记为Ec_control_loop，然后在Package中，通过继承OROCOS的RTT：：TaskContext类，记为Ec_control_loop_component。
(1)在Ec_control_loop_component类的构造函数中，设置为执行如下操作：
a)算法模块利用OROCOS的RTT：：Input与RTT：：Output方法，对输入，输出接口进行定义。
其中输入的接口包括：
①通信管理模块传入的电机运行数据；
②通信管理模块传入的诊断数据；
③设备状态机的状态；
④控制器状态机的状态；
输出的接口包括：
①电机控制指令数据，输出给设备通信模块；
②设备状态机事件触发，输出给设备状态机；
③控制器状态机事件触发，输出给控制器状态机。
b)利用OROCOS的Operational Caller方法设置函数调用接口，设置事件报告的接口：该接口将触发总控模块的事件报告处理函数设置各种运动规划的返回原点指令响应函数。
c)调用OROCOS的Properties方法定义算法模块的属性，算法模块定义一个机械臂关节个数的属性。
(2)在Ec_control_loop_component的StartHook()成员函数中，设置为执行如下操作：
a)检查日志报告是否正常，若异常直接退出，并将相关信息通过事件报告接口传递给总控模块处理；
b)检查电机运行数据通道是否有数据，若无数据直接退出，并将相关信息通过事件报告接口传递给总控模块处理。
(3)对于Ec_control_loop_component类的UpdateHook()成员函数，设置该函数在算法模块运行时，按照用户设定的频率实时运行(如设为100Hz)，可以设置为执行如下操作：
a)读取控制器状态机状态；
b)根据控制器状态机的不同状态，执行不同操作：
Ⅰ、如果是点到点运动，直线运动，圆弧运动，手动示教，急停，回到原点状态。此 时，执行如下操作：
如果指令缓冲区的控制指令个数小于20个，则将所有指令一起发送给通信管理模块，并将控制器状态机的状态改变为等待指令输入状态；
如果指令缓冲区的控制指令个数大于20个，则取指令队列末尾的20个，发送给通信管理模块；
Ⅱ、如果是暂停状态，则什么也不做。
(4)对于Ec_control_loop_component类，定义点到点运动，直线运动，圆弧运动，手动示教，急停，回到原点，暂停，恢复等函数调用接口，实现如下：
a)点到点运动，直线运动，圆弧运动，手动示教，回到原点的函数，内部实现如下：
检查控制器状态机是否处于等待指令输入状态。如果不是则退出，并将相关信息通过事件报告接口传递给总控模块处理；
读取电机的当前的状态信息；
依据电机的当前状态，分别调用点到点运动，直线运动，圆弧运动，手动示教，回到原点的运动规划，并将生成的电机控制指令保存到指令缓冲区；
将控制器状态机设为相应的状态。比如点到点运动回调函数，则将控制器状态机设为点到点运动状态。
b)暂停函数，内部实现如下：
检查控制器状态机是否是点到点运动，直线运动，圆弧运动，手动示教，回到原点等状态。如果不是则退出，并将相关信息通过事件报告接口传递给总控模块进行处理；
记录当前控制器状态机的当前状态，并将控制器状态机的状态转变为暂停状态。
c)恢复函数，内部实现如下：
检查控制器状态机是否是暂停状态。如果不是则退出，并将相关信息通过事件报告接口传递给总控模块处理；
将控制器状态机的状态转变为暂停前的状态。
d)急停函数，内部实现如下：
Ⅰ、检查控制器状态机是否是点到点运动，直线运动，圆弧运动，手动示教，回到原点等状态。如果不是则退出，并将相关信息通过事件报告接口传递给总控模块处理；
Ⅱ、读取电机的当前的状态信息；
Ⅲ、将电机控制指令缓冲区清零；
Ⅳ、调用速度规划运动规划，让电机以最短时间，速度降为0，并将生成的电机控制指令保存到指令缓冲区。
(5)使用Lua语言，编写算法模块的启动文件，设置为执行如下动作：
a)通过OROCOS的import方法，加载算法模块；
d)定义算法模块的刷新频率，线程的优先级别；
c)对算法模块的属性进行赋值；
d)通过OROCOS的connect方法，将算法模块的输入，输出接口和总控模块和通信管理模块的接口建立连接。
e)通过OROCOS的start方法，运行算法模块，算法模块先调用StartHook()函数，然后按设置的刷新频率，实时地周期性调用UpdateHook()函数。
3、对于通信管理模块：
通信管理模块可以通过linux主机minicom中的ttyACM0与Arm开发板通信，可以在该Arm开发板上运行一个CANOpen主站协议，该主站协议可以设置一个指令缓存区，最多可以存储25个指令。
通信管理模块可以利用OROCOS的RTT：：Input与RTT：：Output方法与机器人算法模块和总控模块进行通信。
利用rFSM软件建立设备状态机，对通信管理模块的业务逻辑进行控制。
通信管理模块利用OROCOS的RTT：：Input与RTT：：Output方法与设备状态机连接，可改变设备状态机的状态，并读取状态。
(1)通信模块使用ROS的orocreate-catkin-pkg方法创建为ROS的Package，然后在Package中，通过继承OROCOS的RTT：：TaskContext类，创建一个OROCOS的实时模块，记为Ec_component。
在Ec_component类的构造函数中，设置为执行如下操作：
a)通信管理模块利用OROCOS的RTT：：Input与RTT：：Output方法，对输入，输出接口进行定义。
其中输入的接口包括：
①算法模块传入的控制指令数据；
②设备状态机的状态；
输出的接口包括：
①诊断数据，输出至算法模块和总控模块；
②电机运行数据和机械臂状态数据，输出给算法模块；
③状态机事件触发，输出给设备状态机。
b)利用OROCOS的Operational Caller方法定义函数调用接口，通信管理模块定义事件报告的接口，通过该接口触发总控模块的事件报告处理函数。
c)调用OROCOS的Properties方法定义通信管理模块的属性，通信管理模块定义一个机械臂关节个数的属性。
(2)在Ec_component的StartHook()成员函数中，设置为执行如下操作：
a)检查日志报告是否正常，若异常直接退出，并将相关信息通过事件报告接口传递给总控模块进行处理；
b)电机驱动初始化：
Ⅰ、通过ttyACM0与电机驱动器建立通信；
Ⅱ、电机指令缓冲队列清空；
Ⅲ、电机使能，如果使能成功，则进行下一步，否则退出；
Ⅳ、读取电机的位置，计算机器人的机械臂当前状态，包括关节角度，机械臂末端位姿；
c)机械臂状态初始化：
依据电机位置，判断机械臂是否需要执行回零运动。如果机械臂任一关节角度与零度相差大于0.01度，则执行回零运动，调用点到点运动规划，对回零运动进行规划。
d)改变设备状态机的状态：
如果机器人的机械臂需要执行回零运动，则控制设备状态机保持Init状态不变；否则，向设备状态机发送“e_nominal”事件，将设备状态机的状态转换为Active.Nominal。
(3)对于Ec_component的UpdateHook()成员函数，设置该函数在通信管理模块运行时，以用户设定的频率实时运行(如设为1KHz)，设置为执行如下操作：
a)读取设备状态机状态；
b)根据设备状态机的不同状态，执行不同操作：
Ⅰ、如果是Init状态，执行机械臂的回零运动。此时，执行如下操作：
读取系统的时钟，依据回零运动轨迹规划结果，计算该时刻电机的运动指令，并将该单条运动指令发送给CANOpen主站。
如果运动已回零成功，向设备状态机发送“e_nominal”事件，将设备状态机转换为Active.Nominal，并将该事件报告给总控模块。
Ⅱ、如果是Active.Nominal状态。此时，执行如下操作：
从控制指令数据的输入通道中，读取控制指令，并存储到电机指令缓冲队列中。
读取CANOpen主站指令缓冲区的现有指令个数，如果小于10个，则一次性从电机指令缓冲队列中取出15个运动指令发送给CANOpen主站。如果电机指令缓冲队列的指令个数小于15个，则一次性全部发送给CANOpen主站。
Ⅲ、如果是Active.Recovery状态。此时，通信管理模块处于恢复状态。
此时，依据诊断信息，对系统状态进行恢复，若恢复成功，向设备状态机发送“e_nominal”事件，将设备状态机转换为Active.Nominal。并将该事件报告给总控模块。
若恢复不成功，给状态机发送“e_fault”事件，状态机转换为Fault状态，将该事件报告给总控模块，并直接退出UpdateHook()。
Ⅳ、如果是Active.Halt状态。此时，模块处于暂停状态，执行如下操作：检查控制指令数据的输入通道中是否有新的指令，若有新指令，则读取控制指令，并存储到电机指令缓冲队列中。
Ⅴ、如果是Active.Hands状态。此时，模块处于手控模式，执行如下操作：
读取系统的时钟，依据运动轨迹规划结果，计算该时刻电机的运动指令，并将该单条运动指令发送给CANOpen主站。
Ⅵ、如果是Fault状态，则直接退出UpdateHook()。
c)读取电机状态，根据机械臂的模型计算机械臂关节以及末端运动状态信息，并通过输出数据通道，传递给算法模块与总控模块；
d)检查控制主站是否有错误报告信息，如果有错误信息，则将诊断信息传递给算法模块与总控模块。向设备状态机发送“e_recovery”事件，将设备状态机转换为Active.Recovery状态、并将该事件报告给总控模块。
(4)对于Ec_component的CleanUpHook()成员函数，该函数在模块结束运行时，自动调用，设置执行如下操作：
a)关闭电机驱动使能；
b)关闭电机驱动连接。
(5)参考图9所示，图9是设备状态机的状态变化示意图。共有Init(初始化)、Fault (中断)，Active.Recovery(恢复)、Active.Hands(手动示教)、Active.Halt(暂停)、Active.Nominal(运转)六个状态。Active.Recovery、Active.Hands、Active.Halt、Active.Nominal四个状态组成一个Active(使能)的状态集合，Active的状态转移规则，对四个子状态均有效。
(6)使用Lua语言，编写该模块的启动文件，该启动文件设置执行如下动作：
a)通过OROCOS的import方法，加载通信管理模块；
b)定义通信管理模块的刷新频率，线程的优先级别；
c)对通信管理模块的属性进行赋值；
d)通过OROCOS的connect方法，将通信管理模块的输入，输出接口和总控模块和算法模等的接口建立连接。
e)通过OROCOS的start方法，运行通信管理模块，通信管理模块先调用StartHook()函数，然后按定义好的刷新频率，实时地周期性调用UpdateHook()函数。
对于上述总控模块、算法模块和通信管理模块，设置为当运行后，如果用户需要中途停止该模块，同时按键盘的ctrl键与D键。
综上实施例，基于ROS和OROCOS，利用OROCOS的实时性特点，保证软件程序的实时性能；充分利用ROS的开放性，基于ROS、OROCOS开发的总控模块、算法模块和通信管理模块进行实时通信，共同构成一个完整的机器人控制器软件；进一步建立了控制器状态机和设备状态机，从而实现对总控模块、通信管理模块的业务逻辑进行有效管理。
通过OROCOS的RTT：：Input,RTT：：Output方法建立总控模块、算法模块和通信管理模块的数据输入、输出通道，通过OROCOS的Operational Caller方法定义函数调用接口，通过OROCOS的Properties方法定义总控模块、算法模块和通信管理模块的属性。因此，保证了总控模块、算法模块和通信管理模块之间的独立性与解耦性。
以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (10)

  1. 一种机器人点到点运动控制方法，其特征在于，包括：
    接收控制端传送的点到点运动指令；对点到点运动指令进行解析，获取目标关节角度和点到点运动需要的时间；
    获取机械臂的当前关节角度、当前关节角速度和当前关节角加速度；根据点到点运动需要的时间计算机械臂由当前关节角度运行到目标关节角度经过的第一轨迹，以及分别计算机械臂在所述点到点运动需要的时间内由当前关节角速度和当前关节角加速度降为0所经过的第二轨迹和第三轨迹；根据第一轨迹、第二轨迹和第三轨迹得到点到点的运动轨迹；
    计算机械臂各个关节在所述点到点运动轨迹上运行的各个位置的目标角度、目标角速度和目标角加速度转发至控制主站。
  2. 根据权利要求1所述的机器人点到点运动控制方法，其特征在于，所述计算机械臂各个关节在所述点到点运动轨迹上运行的各个位置的目标角度、目标角速度和目标角加速度的步骤包括：
    在机械臂开始在所述点到点运动轨迹运动后，统计机械臂的运动时间tn；
    若所述运动时间满足：tn≤aT，每隔设定时间τ依据分别计算一次机械臂每个关节的目标角度、目标角速度和目标角加速度；a是系数，T为点到点运动需要的时间；
    若运动时间tn满足：aT<tn<T，将标志位更改为过渡状态，将控制器状态机状态设定为准备状态；
    若运动时间tn满足：tn≥T，点到点运动结束，将标志位更改为等待状态。
  3. 根据权利要求1所述的机器人点到点运动控制方法，其特征在于，所述点到点的运动轨迹计算公式为：
    S(t)＝a0+a1t+a2t2+a3t3+a4t4+a5t5
    a0＝θ0
    Figure PCTCN2017103436-appb-100001
    Figure PCTCN2017103436-appb-100002
    Figure PCTCN2017103436-appb-100003
    Figure PCTCN2017103436-appb-100004
    Figure PCTCN2017103436-appb-100005
    式中，S(t)是五次多项式，t是运行时间；ai，i＝1,…,5是系数。
  4. 根据权利要求3所述的机器人点到点运动控制方法，其特征在于，所述机械臂关节的目标角度、目标角速度和目标角加速度的计算公式为：
    θt＝a0+a1t+a2t2+a3t3+a4t4+a5t5
    Figure PCTCN2017103436-appb-100006
    Figure PCTCN2017103436-appb-100007
    式中，任意时刻t时的关节角位置θt、角速度
    Figure PCTCN2017103436-appb-100008
    和角加速度
    Figure PCTCN2017103436-appb-100009
  5. 根据权利要求2所述的机器人点到点运动控制方法，其特征在于，在过渡状态下，若有新指令输入，则启动过渡运动规划；若没有新指令输入，则每隔设定时间τ计算一次机械臂每个关节的目标角度、目标角速度和目标角加速度，并发送给设备通信软件。
  6. 根据权利要求1所述的机器人点到点运动控制方法，其特征在于，利用预设的通信协议并以异步远程过程调用的方式接收点到点运动指令。
  7. 根据权利要求1所述的机器人点到点运动控制方法，其特征在于，在调用所述点到点运动规划函数后，实时读取机械臂关节的当前状态下的关节角度、关节角速度和关节角加速度，并根据所述关节角度、关节角速度和关节角加速度和点到点运动指令计算点到点的运动轨迹。
  8. 根据权利要求1所述的机器人点到点运动控制方法，其特征在于，还包括：
    在调用点到点运动规划函数前，所述点到点运动执行函数判断控制器状态机是否为准备状态；
    若是，通过OROCOS的Operational Caller方法调用所述点到点运动规划函数，并将控制器状态机切换为执行点到点运动状态；若否，则拒绝执行此次指令。
  9. 根据权利要求8所述的机器人点到点运动控制方法，其特征在于，还包括：
    在调用点到点运动规划函数后，根据所述点到点运动规划函数执行点到点运动规划流程，并检查控制器状态机是否为执行点到点运动状态；若是，执行所述计算点到点的运动轨迹的步骤，否则，退出执行流程。
  10. 一种机器人点到点运动控制系统，其特征在于，包括：总控模块、算法模块和通信管理模块；
    所述总控模块，用于接收控制端传送的点到点运动指令；
    所述算法模块，用于对点到点运动指令进行解析，获取目标关节角度和点到点运动需要的时间；获取机械臂的当前关节角度、当前关节角速度和当前关节角加速度；根据点到 点运动需要的时间计算机械臂由当前关节角度运行到目标关节角度经过的第一轨迹，以及分别计算机械臂在所述点到点运动需要的时间内由当前关节角速度和当前关节角加速度降为0所经过的第二轨迹和第三轨迹；根据第一轨迹、第二轨迹和第三轨迹得到点到点的运动轨迹；以及计算机械臂各个关节在所述点到点运动轨迹上运行的各个位置的目标角度、目标角速度和目标角加速度；
    所述通信管理模块，用于将机械臂各个关节在所述点到点运动轨迹上运行的各个位置的目标角度、目标角速度和目标角加速度转发至控制主站。
PCT/CN2017/103436 2017-04-19 2017-09-26 机器人点到点运动控制方法和系统 WO2018192178A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN201710257078.5A CN106985140B (zh) 2017-04-19 2017-04-19 机器人点到点运动控制方法和系统
CN201710257078.5 2017-04-19

Publications (1)

Publication Number Publication Date
WO2018192178A1 true WO2018192178A1 (zh) 2018-10-25

Family

ID=59415295

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2017/103436 WO2018192178A1 (zh) 2017-04-19 2017-09-26 机器人点到点运动控制方法和系统

Country Status (2)

Country Link
CN (1) CN106985140B (zh)
WO (1) WO2018192178A1 (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113232018A (zh) * 2021-04-27 2021-08-10 成都飞机工业（集团）有限责任公司 一种机器人姿态快速复位的方法

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106985140B (zh) * 2017-04-19 2019-05-07 广州视源电子科技股份有限公司 机器人点到点运动控制方法和系统
CN107263484B (zh) * 2017-08-10 2020-04-14 南京埃斯顿机器人工程有限公司 机器人关节空间点到点运动的轨迹规划方法
CN108037737B (zh) * 2017-12-19 2020-08-18 苏州谷夫道自动化科技有限公司 快速响应的点到点s形速度规划方法
CN110315543B (zh) * 2019-07-29 2021-02-26 北京理工大学 一种双足机器人步态生成与优化方法
CN111890336B (zh) * 2020-06-15 2022-05-10 成都飞机工业（集团）有限责任公司 一种机器人多目标点示教方法、系统

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2636493A2 (en) * 2012-03-09 2013-09-11 Canon Kabushiki Kaisha Information processing apparatus and information processing method
CN103853043A (zh) * 2012-11-30 2014-06-11 北京配天大富精密机械有限公司 机器人中实现同步ptp运动的方法及装置
CN105922265A (zh) * 2016-06-20 2016-09-07 广州视源电子科技股份有限公司 一种机械臂的运动轨迹规划方法、装置及机器人
CN106041941A (zh) * 2016-06-20 2016-10-26 广州视源电子科技股份有限公司 一种机械臂的轨迹规划方法及装置
CN106166749A (zh) * 2016-06-29 2016-11-30 北京控制工程研究所 一种空间移动多臂机器人的移动轨迹规划方法
CN106327952A (zh) * 2016-08-25 2017-01-11 上海新时达电气股份有限公司 示教器的远程控制方法及示教器
CN106985140A (zh) * 2017-04-19 2017-07-28 广州视源电子科技股份有限公司 机器人点到点运动控制方法和系统

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2002066210A1 (fr) * 2001-02-22 2002-08-29 Mitsubishi Denki Kabushiki Kaisha Dispositif de commande de robot
WO2011067621A1 (en) * 2009-12-04 2011-06-09 Toyota Jidosha Kabushiki Kaisha Robot control system, motion data creation apparatus and its creating method
CN102581850B (zh) * 2012-02-10 2014-12-10 广州数控设备有限公司 基于GSK-Link总线的模块化机器人控制装置及控制方法
CN104526695B (zh) * 2014-12-01 2016-03-30 北京邮电大学 一种最小化基座碰撞扰动的空间机械臂轨迹规划方法
CN104699122B (zh) * 2015-02-12 2017-03-08 哈尔滨工业大学 一种机器人运动控制系统
CN105415372B (zh) * 2015-12-09 2017-04-12 常州汉迪机器人科技有限公司 一种安全空间约束下的多关节机器人轨迹规划方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2636493A2 (en) * 2012-03-09 2013-09-11 Canon Kabushiki Kaisha Information processing apparatus and information processing method
CN103853043A (zh) * 2012-11-30 2014-06-11 北京配天大富精密机械有限公司 机器人中实现同步ptp运动的方法及装置
CN105922265A (zh) * 2016-06-20 2016-09-07 广州视源电子科技股份有限公司 一种机械臂的运动轨迹规划方法、装置及机器人
CN106041941A (zh) * 2016-06-20 2016-10-26 广州视源电子科技股份有限公司 一种机械臂的轨迹规划方法及装置
CN106166749A (zh) * 2016-06-29 2016-11-30 北京控制工程研究所 一种空间移动多臂机器人的移动轨迹规划方法
CN106327952A (zh) * 2016-08-25 2017-01-11 上海新时达电气股份有限公司 示教器的远程控制方法及示教器
CN106985140A (zh) * 2017-04-19 2017-07-28 广州视源电子科技股份有限公司 机器人点到点运动控制方法和系统

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113232018A (zh) * 2021-04-27 2021-08-10 成都飞机工业（集团）有限责任公司 一种机器人姿态快速复位的方法
CN113232018B (zh) * 2021-04-27 2022-05-10 成都飞机工业（集团）有限责任公司 一种机器人姿态快速复位的方法

Also Published As

Publication number Publication date
CN106985140B (zh) 2019-05-07
CN106985140A (zh) 2017-07-28

Similar Documents

Publication Publication Date Title
WO2018192178A1 (zh) 机器人点到点运动控制方法和系统
CN106945044B (zh) 机器人暂停运动控制方法和系统
CN106950924B (zh) 机器人圆弧运动控制方法和系统
CN106959694B (zh) 机器人直线运动控制方法和系统
CN106914900B (zh) 基于ros和orocos的机器人控制系统
CN106926243A (zh) 机器人示教运动控制方法和系统
CN109347884B (zh) 一种实时以太网到现场总线的转换方法及装置、存储介质
CN106945045B (zh) 基于ros和orocos的机器人控制的通信方法和系统
CN111427310A (zh) 一种工业机器人控制器运行系统
CN109807903B (zh) 一种机器人控制方法、装置、设备和介质
JP5149258B2 (ja) ロボットコンポーネント管理装置
CN107030695B (zh) 机器人返回原点运动控制方法和系统
JP2024503168A (ja) パラレルロボットのマルチスレッドコントローラ
CN109454641B (zh) 一种运动控制器多任务分工及数据交互方法
WO2018192177A1 (zh) 基于ros和orocos的机器人控制方法和系统
CN106956265B (zh) 基于ros和orocos的机器人运动规划的执行方法和系统
JP3651573B2 (ja) ファクトリーオートメーションシステムの制御方法、ファクトリーオートメーションシステムの中央制御装置
KR100869587B1 (ko) 로봇 미들웨어 프레임워크 시스템
US20220347844A1 (en) Real-time robotics control framework
CN113848812B (zh) 机器人的控制方法、控制装置以及控制系统
US20230405811A1 (en) Extensible hardware abstraction layer for real-time robotics control framework
US20220347846A1 (en) Real-time robotics control framework
US20220347841A1 (en) Real-time robotics control framework
CN111381944A (zh) 基于Android的机器人操作系统及其实现方法
CN117706989A (zh) 基于EtherCAT的工业机器人通用控制及调试系统

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 17906376

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

32PN Ep: public notification in the ep bulletin as address of the adressee cannot be established

Free format text: NOTING OF LOSS OF RIGHTS PURSUANT TO RULE 112(1) EPC (EPO FORM 1205A DATED 26/02/2020)

122 Ep: pct application non-entry in european phase

Ref document number: 17906376

Country of ref document: EP

Kind code of ref document: A1