CN102248536A - Mobile mechanical arm control system used for extendable modularization - Google Patents

Abstract

The invention relates to a control system for an expandable modular mobile robot arm. The solution is: the industrial computer (1) is respectively connected to 1 to 6 manipulator area controllers (4) and 1 to 6 mobile platform area controllers (9) through 1 to 3 motion planning controllers (2); Each manipulator area controller (4) is respectively connected with 1 to 6 joint drive modules (5); each joint drive module (5) is correspondingly connected with the first information collection feedback device (6); every 1 to 6 The first information collection feedback device (6) is connected with the robot arm area controllers (4) connected to the corresponding joint drive modules (5); each mobile platform area controller (9) is respectively connected with 2 to 4 wheel drive modules (8) connection; each wheel drive module (8) is correspondingly connected with the second information collection feedback device (7); every 2 to 4 second information collection feedback devices (7) are respectively corresponding to the wheel drive module (8) The connected mobile platform area controller (9) is connected. The invention has the characteristics of robustness, anti-interference and strong real-time performance.

Description

A control system for scalable modular mobile manipulators

Technical field:

The invention belongs to the technical field of mobile mechanical arm control. In particular, it relates to a control system for an expandable modular mobile manipulator.

Background technique:

In recent years, mobile robotic arms have shown high application value in national defense, industry, agriculture and medical industries. The modular manipulator is composed of various functional modules. Each module is complete and single, making the system flexible, expandable, easy to modify, reconfigure and add configuration functions. Each joint module to achieve the same target pose.

Applying the idea of modularization to the robotic arm, when a module fails, the parameter coupling of other modules can replace the tasks borne by the failed module, which greatly enhances the self-healing ability of the system. Integrated 6-DOF micro-motion parallel robot system (Wang Zhenhua, Chen Liguo, Sun Lining. Integrated 6-DOF micro-motion parallel robot system. Optical Precision Engineering. Volume 15, Issue 9, 2007) has high positioning accuracy and reliability, It is flexible and convenient to use, but it adopts the idea of integrating mechanism, drive and detection. It has a high degree of integration and a closed structure. It is impossible to change the system configuration according to the needs. At the same time, when any unit of the system fails, it cannot complete the task. The robustness of the mobile manipulator system is not high enough.

With the expansion of robot application fields, such as robot operations in dangerous environments, local control can no longer meet the control requirements, and remote operation capability is a must-have requirement for robot systems. Research on modular reconfigurable tracked micro-robots (Li Mantian, Huang Bo, Liu Guocai, Li Ning. Research on Modular Reconfigurable Tracked Micro-robots. Robotics. 2006, Vol. 28, No. 5) used a two-level control system of microcontroller and PC. The structure is reasonable, but Bluetooth communication is used between the two stages. Its working frequency band is the 2.4GHz industrial, scientific and medical frequency band that is unified and open globally. It will be interfered by such as microwave ovens, cordless phones, scientific research instruments, industrial or medical equipment, and has poor anti-interference ability. . At the same time, it also limits the remote control of the robot, and its control flexibility needs to be further improved.

In the motion planning of the joint module of the manipulator, there are a large number of mathematical calculations such as coordinate transformation, solution of forward and reverse kinematic equations, and high-order interpolation operations. At the same time, a large amount of data needs to be exchanged between the motion planning controller and the servo drive, and the real-time requirements for motion planning control need to be met by using a high-speed processor. "Plug and Play Manipulator System Based on Distributed Control" (CN1586829) patent technology adopts distributed control technology. Although this technology improves the control accuracy, it uses 8-bit single-chip microcomputer to complete motion planning control, calculation and data processing capabilities. Limited, the real-time performance of the motion planning controller is affected; the application of embedded systems in robot control systems (Zhou Jie, Chen Weihai, Yu Shouqian. The application of ARM-based embedded systems in robot control systems. Microcomputer Information, 2007 Volume 23, No. 2) technology, although it has a hierarchical architecture, the controller used to complete the control algorithm is ARM, the main frequency is 67.5MHZ, the operation speed is low, and it cannot better meet the real-time requirements.

The mobile manipulator system involves two subsystems, the manipulator and the mobile platform. When the manipulator and the mobile platform are controlled separately and independently, the accuracy will inevitably be affected; when the manipulator and the mobile platform are controlled centrally, there is a strong dynamic coupling between the manipulator subsystem and the mobile platform subsystem, which has a greater impact on the performance of the system. big impact. Therefore, how to coordinate the dynamic obstacle avoidance of the mobile platform and the real-time operation of the end of the mobile manipulator is a crucial issue. "An Intelligent Mobile Robotic Arm Control System" (CN101817182) patent technology adopts distributed control technology. Although this technology can remotely control the autonomous mobile trolley and modular robotic The coordinated control aspect of the robot is not strong, and the advantages of the mobile robot arm that can move and operate at the same time are not fully utilized.

To sum up, the mobile manipulator control system in the prior art has the following problems: the robustness is not strong; the anti-interference is not strong during remote operation; Coordinated control of the platform and the robotic arm is poor.

Invention content:

The purpose of the present invention is to overcome the defects of the existing technology, and the purpose is to provide a mobile robot arm for scalable modularization that is easy to modify, has good robustness, strong coordination control ability, real-time motion planning and strong anti-interference during remote operation Control System.

In order to achieve the above object, the technical solution adopted by the present invention is: the industrial computer is connected to 1 to 3 motion planning controllers in a wired or wireless manner through Ethernet; each motion planning controller is connected to 1 to 3 motion planning controllers respectively through CAN-bus 6 robot arm area controllers are connected to 1~6 mobile platform area controllers; each robot arm area controller is connected to 1~6 joint drive modules through CAN-bus respectively; each joint drive module is connected to its own The corresponding first information collection feedback device is connected; every 1 to 6 first information collection feedback devices are respectively connected to the respective robot arm area controllers connected to the corresponding joint drive modules; each mobile platform area controller is connected via the CAN-bus respectively connected to 2 to 4 wheel drive modules; each wheel drive module is connected to its corresponding second information collection feedback device; every 2 to 4 second information collection feedback devices are respectively connected to their corresponding wheel drive modules Mobile platform area controller connection. The power module is connected with each motion planning controller, each manipulator area controller, each mobile platform area controller, each joint drive module, each wheel drive module, each first information acquisition feedback device and each second The two information collection feedback devices are respectively connected.

The motion planning control software is embedded in the motion planning controller.

The power module includes a DC power supply, a first voltage conversion chip, a second voltage conversion chip and a third voltage conversion chip. The DC power supply is connected to the INPUT port of the first voltage conversion chip, the OUTPUT port of the first voltage conversion chip is connected to the INPUT port of the second voltage conversion chip, and the OUTPUT port of the second voltage conversion chip is connected to the INPUT port of the third voltage conversion chip . The output port OUTPUT of the first voltage conversion chip is connected to each wheel drive module and each joint drive module respectively, and the output port OUTPUT of the second voltage conversion chip is connected to each first information collection feedback device and each second information collection feedback device. The output port OUTPUT of the third voltage conversion chip is respectively connected with each motion planning controller, each manipulator area controller and each mobile platform area controller. The output voltage of the output port OUTPUT of the first voltage conversion chip is +12V, the output voltage of the output port OUTPUT of the second voltage conversion chip is +5V, and the output voltage of the output port OUTPUT of the third voltage conversion chip is +3.3V.

The motion planning controller includes a first DSP chip, an Ethernet controller, an Ethernet connector and a first CAN communication module. XCLKOUT, GPIOB7, XINT1, XINT2, SPISIMO, SPISOMI and SPICLK pins of the first DSP chip are respectively connected to OSC1, The SI, SO and SCK pins are correspondingly connected, the CANTX and CANRX pins of the first DSP chip are respectively connected with the TXD and RXD pins of the first CAN communication module, and the motion planning control software is embedded in the first DSP chip; Ethernet The TPOUT ₊ , TPOUT _- , TPIN ₊ and TPIN _- pins of the controller are connected to the TP_OUT ₊ , TP_OUT _- , TP_IN ₊ and TP_IN _- pins of the Ethernet connector respectively. The VDDIO pin of the first DSP chip, the VDD pin of the Ethernet controller and the TX_CT pin of the Ethernet connector are respectively connected to the power supply module; the first CAN communication module communicates with each robot arm area controller and the Each mobile platform is connected to the regional controller, and the J1-J8 output terminals of the Ethernet connector are connected to the industrial computer through a twisted pair.

The manipulator area controller includes a second DSP chip and a second CAN communication module. The CANTX and CANRX pins of the second DSP chip are respectively connected with the TXD and RXD pins of the second CAN communication module; the IOPA2 and IOPA3 pins of the second DSP chip are respectively connected with each first information collection feedback device; the second The CAN communication module is connected with the motion planning controller and each joint drive module through CAN-bus; the VDDIO pin of the second DSP chip is connected with the power module.

The mobile platform area controller includes a third DSP chip and a third CAN communication module. The CANTX and CANRX pins of the third DSP chip are respectively connected with the TXD and RXD pins of the third CAN communication module; the IOPA2 and IOPA3 pins of the third DSP chip are respectively connected with each second information collection feedback device, and the third The CAN communication module is connected with the motion planning controller and each wheel drive module through CAN-bus; the VDDIO pin of the third DSP chip is connected with the power module.

The joint drive module includes a fourth DSP chip, a first photoelectric coupling module, a first motor drive module and a first self-protection unit. The PWM1 and PWM2 pins of the fourth DSP chip are respectively connected to the ANODE1 and ANODE2 pins of the first photoelectric coupling module, and the IOPE3 pin of the fourth DSP chip is connected to the ENABLE_BRAKE pin of the first self-protection unit. The OV1 and OV2 pins of the coupling module are respectively connected to the BHI and BLI pins of the first motor drive module, and the BRAKE1 and BRAKE2 pins of the first self-protection unit are respectively corresponding to the OUT1 and OUT2 pins of the first motor drive module connect. The fourth DSP chip is connected to the robot arm area controller through the CAN-bus, the APP1_SENSOR and APP2_SENSOR pins of the fourth DSP chip are respectively connected to the corresponding first information collection feedback device, the VDDIO pin of the fourth DSP chip, the first The VCC pin of the photoelectric coupling module and the VCC pin of the first motor driving module are respectively connected to the power module.

The wheel drive module includes a fifth DSP chip, a second photoelectric coupling module, a second motor drive module and a second self-protection unit. The PWM1 and PWM2 pins of the fifth DSP chip are respectively connected to the ANODE1 and ANODE2 pins of the second photoelectric coupling module, the IOPE3 pin of the fifth DSP chip is connected to the ENABLE_BRAKE pin of the second self-protection unit, and the second photoelectric coupling module The OV1 and OV2 pins of the coupling module are respectively connected to the BHI and BLI pins of the second motor drive module, and the BRAKE1 and BRAKE2 pins of the second self-protection unit are respectively corresponding to the OUT1 and OUT2 pins of the second motor drive module connect. The fifth DSP chip is connected with the mobile platform area controller through the CAN-bus; the APP1_SENSOR and APP2_SENSOR pins of the fifth DSP chip are respectively connected with the corresponding second information collection feedback device respectively, the VDDIO pin of the fifth DSP chip, the second The VCC pin of the photoelectric coupling module and the VCC pin of the second motor drive module are respectively connected to the power module.

The first information collection feedback device includes a first hysteresis comparator and a first photoelectric encoder. The 1A and 2A pins of the first hysteresis comparator are respectively connected to the XA and XB pins of the first photoelectric encoder; the 1Y and 2Y pins of the first hysteresis comparator are respectively connected to the robot arm area controller, and The XFSIPUTA and XFSIPUTB pins of a photoelectric encoder are respectively connected to the corresponding joint drive modules, and the VCC pins of the first hysteresis comparator and the VCC pins of the first photoelectric encoder are respectively connected to the power supply module.

The second information collection feedback device includes a second hysteresis comparator and a second photoelectric encoder. The 1A and 2A pins of the second hysteresis comparator are respectively connected with the XA and XB pins of the second photoelectric encoder; the 1Y and 2Y pins of the second hysteresis comparator are respectively connected with the mobile platform area controller, the first The XFSIPUTA and XFSIPUTB pins of the second photoelectric encoder are respectively connected to the corresponding wheel drive modules, and the VCC pins of the second hysteresis comparator and the VCC pins of the second photoelectric encoder are respectively connected to the power supply module.

The main flow of the motion planning control software is:

S-1: Initialize, enter the cycle;

S-2: Determine whether the motion planning controller has received an instruction from the industrial computer, if not, the joints driven by the joint drive module and the wheels driven by the wheel drive module stop moving, and if so, proceed to S-3;

S-3: The motion planning controller sends commands to each robot arm area controller and mobile platform area controller;

S-4: Judging whether the robot arm area controller and the mobile platform area controller have received the instructions from the motion planning controller, if not, re-judgment: if yes, proceed to S-5;

S-5: Determine whether the joint drive module and the wheel drive module are performing zero position detection, if yes, perform zero position detection, and then return to S-4, if not, perform S-6;

S-6: The joint drive module and the wheel drive module respectively receive the corresponding instructions from the respective robot arm area controllers and mobile platform area controllers;

S-7: Read the information received by the joint drive module and the wheel drive module; through the motion control algorithm, the joint drive module controls the joint movement, and the wheel drive module controls the wheel movement;

S-8: Determine whether the joints driven by the joint drive module and the wheels driven by the wheel drive module are working normally. If they work normally, they will send information to the first information collection feedback device and the second information collection feedback device respectively. Start the first self-protection unit and the second self-protection unit, and then send information to the first information collection feedback device and the second information collection feedback device respectively;

S-9: Judging whether the first information collection feedback device and the second information collection feedback device have received the state information of joints and wheels, if yes, go to S-10, if not, return to S-7;

S-10: Send the state information of the joints driven by the joint drive module and the wheels driven by the wheel drive module to the corresponding robot arm area controller and mobile platform area controller, and then return to S-2.

Due to the adoption of the above-mentioned technical scheme, the present invention is formed by splicing various modular units according to a certain topological structure, which reduces the complexity of the entire system; at the same time, the modular design makes the system only need to change and The bus configuration it is connected to greatly reduces the time required for system hardware changes, can easily increase or decrease the degree of freedom of the robot, and enhances the scalability of the system. When the system fails, it is only necessary to find out which module fails, and the module can be processed or replaced accordingly, which makes the maintenance and modification of the system very convenient. At the same time, when a joint fails, the mobile manipulator can adjust itself to make other joints coordinate and control to complete the task, which improves the robustness of the control system of the mobile manipulator.

The invention adopts the CAN-bus communication mode to solve the problems of low transmission efficiency and complex interface circuit in the point-to-point data transmission mode of the traditional controller, which is beneficial to the composition of the mobile manipulator module, has scalability, and improves the control system of the mobile manipulator The anti-jamming performance, especially improves the anti-jamming performance during remote operation.

The invention adopts a hierarchical structure, adding a motion planning controller between the industrial computer, the area controller of the manipulator and the area controller of the mobile platform, and coordinates and controls the area controller of the manipulator and the area controller of the mobile platform through the motion planning controller. The coupling problem between the manipulator subsystem and the mobile platform subsystem is effectively solved, and the advantages of the mobile manipulator's moving and operating functions are fully utilized.

The present invention uses a DSP chip as the core processor, which not only has the advantages of a digital signal processor, but also has the characteristics of a microcontroller, and can meet the high real-time requirements of motion control in terms of computing speed and data processing capacity. Completing complex real-time motion control algorithms provides a reliable platform and also enhances the real-time motion planning capabilities of the mobile manipulator.

The present invention adopts Ethernet to communicate with the upper computer, which not only has significant advantages such as high transmission rate, large amount of information, strong compatibility, flexible and convenient addressing, etc., but also can connect the system to a local area network or the Internet through Ethernet, thereby The remote control of the system is realized, and the flexibility and use value of the system control are improved.

Therefore, the present invention has the characteristics of easy modification, expandability, reconfiguration and configuration addition, good robustness, strong coordination control ability, real-time motion planning and strong anti-interference ability in remote operation. It can be widely used in robot control systems with multiple degrees of freedom modules such as industrial robots, manipulators, space robots, and humanoid robots.

Description of drawings

Fig. 1 is a kind of structural representation of the present invention;

Fig. 2 is a schematic diagram of the circuit connection of the power module 3 in Fig. 1;

Fig. 3 is the circuit connection schematic diagram of motion planning controller 2 among Fig. 1;

Fig. 4 is a schematic diagram of the circuit connection of the manipulator area controller 4 in Fig. 1;

Fig. 5 is a schematic diagram of the circuit connection of the mobile platform area controller 9 in Fig. 1;

Fig. 6 is a schematic diagram of the circuit connection of the joint drive module 5 in Fig. 1;

Fig. 7 is a schematic diagram of the circuit connection of the wheel drive module 8 in Fig. 1;

FIG. 8 is a schematic diagram of the circuit connection of the first information collection feedback device 6 in FIG. 1;

FIG. 9 is a schematic diagram of the circuit connection of the second information collection feedback device 7 in FIG. 1;

Fig. 10 is a schematic diagram of the main flow of the motion planning control software.

Detailed ways

Below in conjunction with accompanying drawing and specific embodiment, the present invention will be further described, not limitation to its protection scope:

Example 1

A control system for scalable modular mobile manipulators. Its structural diagram is shown in Figure 1: industrial computer 1 is connected to a motion planning controller 2 through Ethernet in a wired manner; motion planning controller 2 is connected to two manipulator area controllers 4 and 2 The mobile platform area controller 9 is connected; each manipulator area controller 4 is respectively connected to two joint drive modules 5 through CAN-bus; each joint drive module 5 is respectively connected to the corresponding first information collection feedback device 6 ; Every two first information collection feedback devices 6 are respectively connected to the robot arm area controllers 4 connected to the corresponding joint drive modules 5 . Each mobile platform area controller 9 is respectively connected with 2 wheel drive modules 8 by CAN-bus; each wheel drive module 8 is connected with the corresponding second information collection feedback device 7 respectively; The feedback devices 7 are respectively connected to the mobile platform area controllers 9 connected to the corresponding wheel drive modules 8 . Power supply module 3 and motion planning controller 2, each robotic arm area controller 4, each mobile platform area controller 9, each joint drive module 5, each wheel drive module 8, and each first information collection feedback device 6 and each second information collection feedback device 7 are respectively connected.

The motion planning control software is embedded in the motion planning controller 2 .

The power module 3 includes a DC power supply 13 , a first voltage conversion chip 12 , a second voltage conversion chip 11 and a third voltage conversion chip 10 . The DC power supply 13 is connected to the INPUT port of the first voltage conversion chip 12, the OUTPUT port of the first voltage conversion chip 12 is connected to the INPUT port of the second voltage conversion chip 11, and the OUTPUT port of the second voltage conversion chip 11 is connected to the third voltage conversion The INPUT port of the chip 10 is connected. The output port OUTPUT of the first voltage conversion chip 12 is connected with each wheel drive module 8 and each joint drive module 5 respectively, and the output port OUTPUT of the second voltage conversion chip 11 is connected with each first information collection feedback device 6 and each The second information collection feedback device 7 is connected respectively, and the output port OUTPUT of the third voltage conversion chip 10 is connected with each motion planning controller 2 , each manipulator area controller 4 and each mobile platform area controller 9 respectively. The output voltage of the output port OUTPUT of the first voltage conversion chip 12 is +12V, the output voltage of the output port OUTPUT of the second voltage conversion chip 11 is +5V, and the output voltage of the output port OUTPUT of the third voltage conversion chip 10 is +3.3V. V.

SI、SO和SCK引脚对应连接，第一DSP芯片14的CANTX和CANRX引脚分别与第一CAN通讯模块17的TXD和RXD引脚对应连接，运动规划控制软件嵌入在第一DSP芯片14中；以太网控制器15的TPOUT₊、TPOUT_-、TPIN₊和TPIN_-引脚分别与以太网连接器16的TP_OUT₊、TP_OUT_-、TP_IN₊和TP_IN_-引脚对应连接。第一DSP芯片14的VDDIO引脚、以太网控制器15的VDD引脚和以太网连接器16的TX_CT引脚分别与电源模块3连接；第一CAN通讯模块17通过CAN-bus与每个机械臂区域控制器4和每个移动平台区域控制器9连接，以太网连接器16的J1～J8输出端通过双绞线与工业计算机1连接。The motion planning controller 2 includes a first DSP chip 14 , an Ethernet controller 15 , an Ethernet connector 16 and a first CAN communication module 17 . XCLKOUT, GPIOB7, XINT1, XINT2, SPISIMO, SPISOMI and SPICLK pins of the first DSP chip 14 are respectively connected to OSC1,

The SI, SO and SCK pins are correspondingly connected, the CANTX and CANRX pins of the first DSP chip 14 are respectively connected with the TXD and RXD pins of the first CAN communication module 17, and the motion planning control software is embedded in the first DSP chip 14 ; The TPOUT ₊ , _TPOUT- , TPIN ₊ and TPIN _- pins of the Ethernet controller 15 are connected to the TP_OUT ₊ , TP_OUT- _, TP_IN ₊ and TP_IN _- pins of the Ethernet connector 16 respectively. The VDDIO pin of the first DSP chip 14, the VDD pin of the Ethernet controller 15 and the TX_CT pin of the Ethernet connector 16 are connected with the power supply module 3 respectively; The arm area controller 4 is connected to each mobile platform area controller 9, and the J1-J8 output ends of the Ethernet connector 16 are connected to the industrial computer 1 through a twisted pair.

The manipulator area controller 4 includes a second DSP chip 18 and a second CAN communication module 19 . CANTX and CANRX pins of the second DSP chip 18 are correspondingly connected with the TXD and RXD pins of the second CAN communication module 19 respectively; Connection; the second CAN communication module 19 is connected with the motion planning controller 2 and each joint drive module 5 through CAN-bus; the VDDIO pin of the second DSP chip 18 is connected with the power module 3 .

The mobile platform regional controller 9 includes a third DSP chip 20 and a third CAN communication module 21 . The CANTX and CANRX pins of the third DSP chip 20 are respectively connected to the TXD and RXD pins of the third CAN communication module 21; Connection, the third CAN communication module 21 is connected with the motion planning controller 2 and each wheel drive module 8 through CAN-bus; the VDDIO pin of the third DSP chip 20 is connected with the power module 3 .

The joint driving module 5 includes a fourth DSP chip 22 , a first photoelectric coupling module 23 , a first motor driving module 24 and a first self-protection unit 25 . The PWM1 and PWM2 pins of the fourth DSP chip 22 are respectively connected to the ANODE1 and ANODE2 pins of the first photoelectric coupling module 23, and the IOPE3 pin of the fourth DSP chip 22 is connected to the ENABLE BRAKE pin of the first self-protection unit 25 connection, the OV1 and OV2 pins of the first photoelectric coupling module 23 are respectively connected to the BHI and BLI pins of the first motor drive module 24, and the BRAKE1 and BRAKE2 pins of the first self-protection unit 25 are respectively connected to the first motor drive The OUT1 and OUT2 pins of the module 24 are connected correspondingly. The fourth DSP chip 22 is connected to the manipulator area controller 4 through the CAN-bus, the APP1_SENSOR and APP2_SENSOR pins of the fourth DSP chip 22 are respectively connected to the corresponding first information collection feedback device 6, and the VDDIO of the fourth DSP chip 22 pin, the VCC pin of the first photocoupler module 23 and the VCC pin of the first motor drive module 24 are respectively connected to the power module 3 .

The wheel drive module 8 includes a fifth DSP chip 26 , a second photocoupler module 27 , a second motor drive module 28 and a second self-protection unit 29 . The PWM1 and PWM2 pins of the fifth DSP chip 26 are respectively connected to the ANODE1 and ANODE2 pins of the second photoelectric coupling module 27, and the IOPE3 pin of the fifth DSP chip 26 is connected to the ENABLE_BRAKE pin of the second self-protection unit 29 , the OV1 and OV2 pins of the second photoelectric coupling module 27 are respectively connected to the BHI and BLI pins of the second motor drive module 28, and the BRAKE1 and BRAKE2 pins of the second self-protection unit 29 are respectively connected to the second motor drive module The OUT1 and OUT2 pins of the 28 are connected correspondingly. The fifth DSP chip 26 is connected with the mobile platform area controller 9 through CAN-bus; the APP1_SENSOR and APP2_SENSOR pins of the fifth DSP chip 26 are respectively connected with the corresponding second information collection feedback device 7 respectively, and the VDDIO of the fifth DSP chip 26 pin, the VCC pin of the second photocoupling module 27 and the VCC pin of the second motor driving module 28 are respectively connected to the power module 3 .

The first information collection feedback device 6 includes a first hysteresis comparator 30 and a first photoelectric encoder 31 . The 1A and 2A pins of the first hysteresis comparator 30 are correspondingly connected with the XA and XB pins of the first photoelectric encoder 31 respectively; 4 connections, the XFSIPUTA and XFSIPUTB pins of the first photoelectric encoder 31 are connected to the corresponding joint drive modules 5 respectively, and the VCC pins of the first hysteresis comparator 30 and the VCC pins of the first photoelectric encoder 31 are respectively connected with The power module 3 is connected.

The second information collection feedback unit 7 includes a second hysteresis comparator 32 and a second photoelectric encoder 33 . The 1A and 2A pins of the second hysteresis comparator 32 are correspondingly connected with the XA and XB pins of the second photoelectric encoder 33 respectively; the 1Y and 2Y pins of the second hysteresis comparator 32 are respectively connected with the mobile platform area controller 9 connection, the XFSIPUTA and XFSIPUTB pins of the second photoelectric encoder 33 are connected with the corresponding wheel drive module 8 respectively, the VCC pins of the second hysteresis comparator 32 and the VCC pins of the second photoelectric encoder 33 are respectively connected with The power module 3 is connected.

The main flow of the motion planning control software is:

S-1: Initialize, enter the cycle;

S-2: Determine whether the motion planning controller 2 has received the instruction from the industrial computer 1, if not, the joints driven by the joint drive module 5 and the wheels driven by the wheel drive module 8 stop moving, and if so, proceed to S-3;

S-3: The motion planning controller 2 sends instructions to each robot arm area controller 4 and mobile platform area controller 9;

S-4: Judging whether the manipulator area controller 4 and the mobile platform area controller 9 have received the instruction from the motion planning controller 2, if not, re-judgment: if yes, proceed to S-5;

S-5: Determine whether the joint drive module 5 and the wheel drive module 8 perform zero position detection, if yes, perform zero position detection, and then return to S-4, if not, perform S-6;

S-6: the joint drive module 5 and the wheel drive module 8 respectively receive corresponding instructions from the corresponding robot arm area controller 4 and the mobile platform area controller 9;

S-7: Read the information received by the joint drive module 5 and the wheel drive module 8; through the motion control algorithm, the joint drive module 5 controls the joint action, and the wheel drive module 8 controls the wheel movement;

S-8: Determine whether the joints driven by the joint drive module 5 and the wheels driven by the wheel drive module 8 are working normally, and if they are working normally, send information to the first information collection feedback device 6 and the second information collection feedback device 7 respectively. Normal operation then starts the first self-protection unit 25 and the second self-protection unit 29 respectively, and then sends information to the first information collection feedback unit 6 and the second information collection feedback unit 7 respectively;

S-9: Judging whether the first information collection feedback device 6 and the second information collection feedback device 7 have received the status information of joints and wheels, if yes, proceed to S-10, if not, return to S-7;

S-10: Send the status information of the joints driven by the joint drive module 5 and the wheels driven by the wheel drive module 8 to the corresponding manipulator area controller 4 and mobile platform area controller 9, and then return to S-2.

Example 2

A control system for scalable modular mobile manipulators. The industrial computer 1 is connected to two motion planning controllers 2 in a wireless manner via Ethernet; each motion planning controller 2 is connected to 3 to 6 manipulator area controllers 4 and 3 to 6 mobile controllers respectively through CAN-bus. The platform area controller 9 is connected; each manipulator area controller 4 is respectively connected to a joint drive module 5 through CAN-bus; each joint drive module 5 is respectively connected to the corresponding first information collection feedback device 6; Every 3-6 first information collection and feedback devices 6 are respectively connected to the manipulator area controllers 4 connected to the corresponding joint drive modules 5 . Each mobile platform area controller 9 is respectively connected with a wheel drive module 8 through CAN-bus; each wheel drive module 8 is connected with the corresponding second information collection feedback device 7 respectively; The information collection feedback devices 7 are respectively connected to the mobile platform area controllers 9 connected to the corresponding wheel drive modules 8 . The power supply module 3 and each motion planning controller 2, each manipulator area controller 4, each mobile platform area controller 9, each joint drive module 5, each wheel drive module 8, each first information collection The feedback unit 6 is connected to each second information collection feedback unit 7 respectively.

All the other are with embodiment 1.

Example 3

A control system for scalable modular mobile manipulators. The industrial computer 1 is connected to the three motion planning controllers 2 in a wireless manner through Ethernet; each motion planning controller 2 is respectively connected to a robot arm area controller 4 and a mobile platform area controller through CAN-bus 9 connections; each robotic arm area controller 4 is respectively connected to a joint drive module 5 via CAN-bus; each joint drive module 5 is connected to its corresponding first information collection feedback device 6; each first The information collection and feedback devices 6 are respectively connected to the robot arm area controllers 4 connected to the corresponding joint drive modules 5 . Each mobile platform area controller 9 is respectively connected with 1 wheel drive module 8 by CAN-bus; Each wheel drive module 8 is connected with respectively corresponding second information collection feedback device 7 respectively; Device 7 is respectively connected with mobile platform area controller 9 that respectively corresponding wheel drive module 8 is connected. The power supply module 3 and each motion planning controller 2, each manipulator area controller 4, each mobile platform area controller 9, each joint drive module 5, each wheel drive module 8, each first information collection The feedback unit 6 is connected to each second information collection feedback unit 7 respectively.

All the other are with embodiment 1.

This specific implementation mode is composed of various modular units spliced according to a certain topology structure, which reduces the complexity of the entire system; at the same time, the modular design makes the system only need to change the bus connected to it when the hardware changes. The configuration greatly reduces the time required for system hardware changes, and can easily increase or decrease the degree of freedom of the robot, enhancing the scalability of the system. When the system fails, it is only necessary to find out which module fails, and the module can be processed or replaced accordingly, which makes the maintenance and modification of the system very convenient. At the same time, when a joint fails, the mobile manipulator can adjust itself to make other joints coordinate and control to complete the task, which improves the robustness of the control system of the mobile manipulator.

This specific implementation adopts the CAN-bus communication method to solve the problems of low transmission efficiency and complex interface circuits in the point-to-point data transmission method of the traditional controller, which is beneficial to the composition of the mobile manipulator module, and has scalability, and improves the mobile manipulator. The anti-jamming performance of the control system, especially improves the anti-jamming performance during remote operation.

This specific embodiment adopts a hierarchical structure, and a motion planning controller 2 is added between the industrial computer 1 and the manipulator area controller 4 and the mobile platform area controller 9, and the manipulator area controller 4 and the manipulator area controller 4 are controlled by the motion planning controller 2 The coordinated control of the mobile platform area controller 9 effectively solves the coupling problem between the manipulator subsystem and the mobile platform subsystem, and gives full play to the advantages of the mobile manipulator's moving and operating functions.

This specific embodiment adopts the DSP chip as the core processor, which not only has the advantages of a digital signal processor, but also has the characteristics of a microcontroller, and can meet the high real-time requirements of motion control in terms of computing speed and data processing capacity. , which provides a reliable platform for completing complex real-time motion control algorithms, and also enhances the real-time motion planning capability of the mobile manipulator.

This specific implementation mode uses Ethernet to communicate with the upper computer, which not only has significant advantages such as high transmission rate, large amount of information, strong compatibility, flexible and convenient addressing, etc., but also can connect the system to a local area network or the Internet through Ethernet , so as to realize the remote control of the system and improve the flexibility and use value of the system control.

Therefore, this specific embodiment has the characteristics of easy modification, expandability, reconfiguration and configuration addition, good robustness, strong coordination control ability, real-time motion planning and strong anti-interference ability in remote operation. It can be widely used in robot control systems with multiple degrees of freedom modules such as industrial robots, manipulators, space robots, and humanoid robots.

Claims (10)

1. one kind is used for expanding modular moving mechanical arm control system, it is characterized in that: industrial computer (1) is connected respectively with 1～3 apparatus for controlling movement programming (2) in wired or wireless mode by Ethernet; Each apparatus for controlling movement programming (2) is connected with 1～6 mobile platform zone controller (9) with 1～6 mechanical arm zone controller (4) respectively by CAN-bus separately; Each mechanical arm zone controller (4) is connected with 1～6 joint driver module (5) respectively by CAN-bus separately; Each joint driver module (5) is gathered ultramagnifier (6) with each self-corresponding first information respectively and is connected; Per 1～6 first information is gathered mechanical arm zone controller (4) connection that ultramagnifier (6) is connected with each self-corresponding joint driver module (5) respectively; Each mobile platform zone controller (9) is connected with 2～4 wheel driver modules (8) respectively by CAN-bus separately; Each wheel driver module (8) is connected with each self-corresponding second information gathering ultramagnifier (7) respectively; The mobile platform zone controller (9) that per 2～4 second information gathering ultramagnifiers (7) are connected with each self-corresponding wheel driver module (8) respectively connects; Power module (3) is gathered ultramagnifier (6) with each apparatus for controlling movement programming (2), each mechanical arm zone controller (4), each mobile platform zone controller (9), each joint driver module (5), each wheel driver module (8), each first information and is connected respectively with each second information gathering ultramagnifier (7);

Motion planning control software is embedded in the apparatus for controlling movement programming (2).

2. according to claim 1 being used for can be expanded modular moving mechanical arm control system, it is characterized in that described power module (3) comprises dc source (13), the first voltage transitions chip (12), the second voltage transitions chip (11) and tertiary voltage conversion chip (10); Dc source (13) is connected with the INPUT port of the first voltage transitions chip (12), the OUTPUT port of the first voltage transitions chip (12) is connected with the INPUT port of the second voltage transitions chip (11), and the OUTPUT port of the second voltage transitions chip (11) is connected with the INPUT port of tertiary voltage conversion chip (10);

The output port OUTPUT of the first voltage transitions chip (12) is connected respectively with each joint driver module (5) with each wheel driver module (8), the output port OUTPUT of the second voltage transitions chip (11) gathers ultramagnifier (6) with each first information and is connected respectively with each second information gathering ultramagnifier (7), and the output port OUTPUT of tertiary voltage conversion chip (10) is connected respectively with each apparatus for controlling movement programming (2), each mechanical arm zone controller (4) and each mobile platform zone controller (9);

The output voltage of the output port OUTPUT of the first voltage transitions chip (12) is+12V, the output voltage of the output port OUTPUT of the second voltage transitions chip (11) is+5V that the output voltage of the output port OUTPUT of tertiary voltage conversion chip (10) is+3.3V.

3. according to claim 1 being used for can be expanded modular moving mechanical arm control system, it is characterized in that described apparatus for controlling movement programming (2) comprises first dsp chip (14), ethernet controller (15), ethernet connector (16) and a CAN communication module (17); XCLKOUT, the GPIOB7 of first dsp chip (14), XINT1, XINT2, SPISIMO, SPISOMI and SPICLK pin respectively with the OSC1 of ethernet controller (15),

SI, SO are connected with the SCK pin is corresponding, and the CANTX of first dsp chip (14) is connected with the RXD pin is corresponding with the TXD of a CAN communication module (17) respectively with the CANRX pin, and motion planning control software is embedded in first dsp chip (14); The TPOUT of ethernet controller (15) ₊, TPOUT _-, TPIN ₊And TPIN _-Pin respectively with the TP_OUT of ethernet connector (16) ₊, TP_OUT _-, TP_IN ₊And TP_IN _-Pin is corresponding to be connected;

The TX_CT pin of the VDD pin of the VDDIO pin of first dsp chip (14), ethernet controller (15) and ethernet connector (16) is connected with power module (3) respectively; The one CAN communication module (17) is connected with each mobile platform zone controller (9) with each mechanical arm zone controller (4) by CAN-bus, and the J1 of ethernet connector (16)～J8 output is connected with industrial computer (1) by twisted-pair feeder.

4. according to claim 1 being used for can be expanded modular moving mechanical arm control system, it is characterized in that described mechanical arm zone controller (4) comprises second dsp chip (18) and the 2nd CAN communication module (19); The CANTX of second dsp chip (18) is connected with the RXD pin is corresponding with the TXD of the 2nd CAN communication module (19) respectively with the CANRX pin;

The IOPA2 of second dsp chip (18) gathers ultramagnifier (6) with each first information respectively with the IOPA3 pin and is connected; The 2nd CAN communication module (19) is connected with each joint driver module (5) with apparatus for controlling movement programming (2) by CAN-bus; The VDDIO pin of second dsp chip (18) is connected with power module (3).

5. according to claim 1 being used for can be expanded modular moving mechanical arm control system, it is characterized in that described mobile platform zone controller (9) comprises the 3rd dsp chip (20) and the 3rd CAN communication module (21); The CANTX of the 3rd dsp chip (20) is connected with the RXD pin is corresponding with the TXD of the 3rd CAN communication module (21) respectively with the CANRX pin;

The IOPA2 of the 3rd dsp chip (20) is connected with each second information gathering ultramagnifier (7) respectively with the IOPA3 pin, and the 3rd CAN communication module (21) is connected with each wheel driver module (8) with apparatus for controlling movement programming (2) by CAN-bus; The VDDIO pin of the 3rd dsp chip (20) is connected with power module (3).

6. according to claim 1 being used for can be expanded modular moving mechanical arm control system, it is characterized in that described joint driver module (5) comprises the 4th dsp chip (22), the first photoelectricity coupling module (23), first motor drive module (24) and the first self-protection unit (25); The PWM1 of the 4th dsp chip (22) is connected with the ANODE2 pin is corresponding with the ANODE1 of the first photoelectric coupling module (23) respectively with the PWM2 pin, the IOPE3 pin of the 4th dsp chip (22) is connected with the ENABLE_BRAKE pin of the first self-protection unit (25), the OV1 of the first photoelectric coupling module (23) is connected with the BLI pin is corresponding with the BHI of first motor drive module (24) respectively with the OV2 pin, and the BRAKE1 of the first self-protection unit (25) is connected with the OUT2 pin is corresponding with the OUT1 of first motor drive module (24) respectively with the BRAKE2 pin;

The 4th dsp chip (22) is connected with mechanical arm zone controller (4) by CAN-bus, the APP1_SENSOR of the 4th dsp chip (22) gathers ultramagnifier (6) with each self-corresponding first information respectively with the APP2_SENSOR pin and is connected, and the VCC pin of the VDDIO pin of the 4th dsp chip (22), the first photoelectricity coupling module (23) is connected with power module (3) respectively with the VCC pin of first motor drive module (24).

7. according to claim 1 being used for can be expanded modular moving mechanical arm control system, it is characterized in that described wheel driver module (8) comprises the 5th dsp chip (26), the second photoelectricity coupling module (27), second motor drive module (28) and alter ego protected location (29); The PWM1 of the 5th dsp chip (26) is connected with the ANODE2 pin is corresponding with the ANODE1 of the second photoelectric coupling module (27) respectively with the PWM2 pin, the IOPE3 pin of the 5th dsp chip (26) is connected with the ENABLE_BRAKE pin of alter ego protected location (29), the OV1 of the second photoelectric coupling module (27) is connected with the BLI pin is corresponding with the BHI of second motor drive module (28) respectively with the OV2 pin, and the BRAKE1 of alter ego protected location (29) is connected with the OUT2 pin is corresponding with the OUT1 of second motor drive module (28) respectively with the BRAKE2 pin;

The 5th dsp chip (26) is connected with mobile platform zone controller (9) by CAN-bus; The APP1_SENSOR of the 5th dsp chip (26) is connected with each self-corresponding second information gathering ultramagnifier (7) respectively with the APP2_SENSOR pin, and the VCC pin of the VDDIO pin of the 5th dsp chip (26), the second photoelectricity coupling module (27) is connected with power module (3) respectively with the VCC pin of second motor drive module (28).

8. according to claim 1 being used for can be expanded modular moving mechanical arm control system, it is characterized in that described first information collection ultramagnifier (6) comprises first hysteresis loop comparator (30) and first photoelectric encoder (31); The 1A of first hysteresis loop comparator (30) is connected with the XB pin is corresponding with the XA of first photoelectric encoder (31) respectively with the 2A pin;

The 1Y of first hysteresis loop comparator (30) is connected with mechanical arm zone controller (4) respectively with the 2Y pin, the XFSIPUTA of first photoelectric encoder (31) is connected with each self-corresponding joint driver module (5) respectively with the XFSIPUTB pin, and the VCC pin of the VCC pin of first hysteresis loop comparator (30) and first photoelectric encoder (31) is connected with power module (3) respectively.

9. according to claim 1 being used for can be expanded modular moving mechanical arm control system, it is characterized in that the described second information gathering ultramagnifier (7) comprises second hysteresis loop comparator (32) and second photoelectric encoder (33); The 1A of second hysteresis loop comparator (32) is connected with the XB pin is corresponding with the XA of second photoelectric encoder (33) respectively with the 2A pin;

The 1Y of second hysteresis loop comparator (32) is connected with mobile platform zone controller (9) respectively with the 2Y pin, the XFSIPUTA of second photoelectric encoder (33) is connected with each self-corresponding wheel driver module (8) respectively with the XFSIPUTB pin, and the VCC pin of the VCC pin of second hysteresis loop comparator (32) and second photoelectric encoder (33) is connected with power module (3) respectively.

10. can expand modular moving mechanical arm control system according to claim 1 or 3 described being used for, it is characterized in that the main flow of described motion planning control software is:

S-1: initialization enters circulation;

S-2: judging whether apparatus for controlling movement programming (2) receives the instruction of industrial computer (1), is not the joint of then joint driver module (5) driving and the wheel stop motion that wheel driver module (8) drives, and is then to carry out S-3;

S-3: apparatus for controlling movement programming (2) sends instruction to each mechanical arm zone controller (4) and mobile platform zone controller (9);

S-4: judging whether mechanical arm zone controller (4) and mobile platform zone controller (9) receive the instruction of apparatus for controlling movement programming (2), is not then to rejudge: be then to carry out S-5;

S-5: judging whether joint driver module (5) and wheel driver module (8) carry out zero position and detect, is then to carry out zero position to detect, and returns S-4 again, is not then to carry out S-6;

S-6: joint driver module (5) and wheel driver module (8) receive each self-corresponding mechanical arm zone controller (4) and mobile platform zone controller (9) corresponding instruction respectively;

S-7: read the information that joint driver module (5) and wheel driver module (8) receive; By motion control arithmetic, joint driver module (5) control joint action, the motion of wheel driver module (8) control wheel;

S-8: judge whether operate as normal of wheel that joint that joint driver module (5) is driven and wheel driver module (8) driven, operate as normal then sends information to the first information respectively and gathers the ultramagnifier (6) and the second information gathering ultramagnifier (7), non-normal working then starts the first self-protection unit (25) and alter ego protected location (29) respectively, sends information to the first information more respectively and gathers the ultramagnifier (6) and the second information gathering ultramagnifier (7);

S-9: judge whether the first information collection ultramagnifier (6) and the second information gathering ultramagnifier (7) receive the status information of joint and wheel, are then to carry out S-10, are not then to return S-7;

S-10: the status information of the wheel that joint that transmission joint driver module (5) is driven and wheel driver module (8) are driven is returned S-2 again to corresponding mechanical arm zone controller (4) and mobile platform zone controller (9).

Images