Multi-DOF Robotic Arm Drive Controller

Abstract

The present invention discloses a multi-degree-of-freedom mechanical arm drive controller, and relates to the technical field of mechanical arm control systems. The present invention includes a processor module and a motor drive module, the processor module includes an ARM processing module, a clock reset signal processing module and a bus arbitration module, the ARM processing module is electrically connected to the clock reset signal processing module, the clock reset signal processing module is electrically connected to the bus arbitration module, and the bus arbitration module is connected to the motor drive module; the motor drive module includes a motor drive bus communication module, a signal line shunt and convergence related IP module, a robot pulse drive module 1, a robot pulse drive module 2 and an emergency stop protection module, and the motor drive bus communication module is electrically connected to the signal shunt and convergence related IP module. The multi-degree-of-freedom mechanical arm drive controller of the present invention is based on the stepper motor drive IP of FPGA to realize drive control integration.

Description

Technical Field

[0001] The invention belongs to the technical field of robot control systems, and in particular relates to a multi-degree-of-freedom robot drive controller. Background Art

[0002] The integrated drive and control system has gained wide attention due to its advantages of small size, flexible deployment and low cost. The integrated drive and control system requires resources such as high-performance computers, high-speed information exchange, sufficient storage space, and rich peripheral interfaces. The selection of the core processor is the focus of system design. Embedded systems have become the first choice for integrated drive and control systems due to their convenient development, strong scalability, and high real-time performance. The MCU has a low main frequency and poor data processing capabilities, and is not suitable for systems with high computing performance requirements. DSP can realize complex algorithms such as digital filtering and digital spectrum analysis by adding multipliers and optimizing memory structure, and occupies a large market in high-performance control systems. SoC starts from the system performance and overall functional level, relying on software and hardware collaborative design, IP module reuse and comprehensive analysis and verification technology to improve embedded system performance and reduce development costs.

[0003] The physical carriers of SoC include Application Specific Integrated Circuit (ASIC) and Field Programmable Gate Array (FPGA).

[0004] ASIC processors have high performance, but the system design process is complex and the development efficiency is low; FPGA has relatively low performance per unit area, but the system implementation cycle is short and the development and verification efficiency is higher. Most drive-control systems use a distributed structure of ARM+DSP+FPGA. The integration and reliability of this solution are mainly affected by chip layout, circuit design, signal stability, etc. With the improvement of FPGA performance and the introduction of new design solutions, high-performance motion control systems can be achieved only by FPGA. Summary of the invention

[0005] The purpose of the present invention is to provide a multi-degree-of-freedom robotic arm drive controller, which adopts an FPGA-based SoC device to design a drive-control integrated system, which can effectively improve the influence of the above factors.

[0006] In order to solve the above technical problems, the present invention is achieved through the following technical solutions:

[0007] The present invention is a multi-degree-of-freedom mechanical arm drive controller, comprising a processor module and a motor drive module, the processor module comprising an ARM processing module, a clock reset signal processing module and a bus arbitration module, the ARM processing module is electrically connected to the clock reset signal processing module, the clock reset signal processing module is electrically connected to the bus arbitration module, and the bus arbitration module is connected to the motor drive module;

[0008] The motor drive module includes a motor drive bus communication module, a signal line shunt and convergence related IP module, a robot pulse drive module 1, a robot pulse drive module 2 and an emergency stop protection module. The motor drive bus communication module is electrically connected to the signal shunt and convergence related IP module, and the signal shunt and convergence related IP module is electrically connected to the robot pulse drive module 1 and the robot pulse drive module 2 respectively. The robot pulse drive module 1 and the robot pulse drive module 2 are both electrically connected to the emergency stop protection module.

[0009] Preferably, the ARM processing module is responsible for high-level abstract work such as the algorithm software part, and is connected to the bus arbitration module.

[0010] Preferably, the bus arbitration module provides bus arbitration services for other acceleration peripheral IPs.

[0011] Preferably, the clock reset signal processing module receives the input reset signal and clock signal, and generates a synchronous reset signal to reset the peripheral safely.

[0012] Preferably, the robot pulse driving module 1 and the robot pulse driving module 2 are robot pulse driving IPs, and the robot pulse driving module 1 and the robot pulse driving module 2 are each responsible for the driving pulse of a six-degree-of-freedom robotic arm.

[0013] Preferably, the emergency stop protection module is responsible for receiving the emergency stop protection level signal from the external hardware circuit.

[0014] Preferably, the signal line branching and converging related IP modules include a bus communication IP, which is responsible for receiving control signals from the processor and converting the control signals into control commands and sending them to other IPs.

[0015] Preferably, the six-degree-of-freedom robotic arm has no less than six rotating joints driven by stepper motors.

[0016] Preferably, the multi-degree-of-freedom robot drive controller adopts the following steps to perform motion planning of the multi-degree-of-freedom robot in a static environment:

[0017] Step S01: obtaining a large number of posture points in the configuration space of the robot arm by random sampling, which are random intermediate posture points;

[0018] Step S02: determining whether the random intermediate posture points are legal, and retaining the legal random intermediate posture points;

[0019] Step S03: traverse all posture points and connect random intermediate posture points that meet the requirements to form a path;

[0020] Step S04: Check the formed path to ensure that it will not collide with the static environment or the robot itself, and form a legal path set;

[0021] Step S05: Execute motion planning offline in common working scenarios of future systems and count the usage frequencies of all paths;

[0022] Step S06: Filter a specified number of paths from the legal path set for encoding the collision grid, giving priority to those with high frequency of use. The number of paths is determined by the system's requirements for success rate;

[0023] Step S07: dividing the space into a number of grids according to the coordinates of the coordinate system;

[0024] Step S08: executing each path separately in an offline environment, and recording the grids touched during the execution of the path;

[0025] Step S09: construct CDU, and generate a truth table for each path according to the collision data obtained in the above steps;

[0026] Step S10: Acquire sensor data to obtain an occupancy grid of an obstacle;

[0027] Step S11: taking the grid of obstacles as the input of the truth table and obtaining the output;

[0028] Step S12: Use a graph theory-based search algorithm to search for a route from the starting posture point to the target posture point.

[0029] Preferably, S01-S09 are system building steps and are completed in an offline state; steps S10-S12 are system execution steps and are completed in an online state; the grid of step S07 includes three types, namely:

[0030] 1. Areas that the robot can reach and where obstacles will appear;

[0031] Second, the area where the robot arm can reach but obstacles will not appear;

[0032] 3. Obstacles will appear, but the robot arm cannot reach the area;

[0033] In step S11, the result of the output truth table is two situations: the path collides with the current obstacle or the path is still safe.

[0034] The present invention has the following beneficial effects:

[0035] The multi-degree-of-freedom mechanical arm drive controller of the present invention is based on the stepper motor drive IP of FPGA to achieve drive control integration.

[0036] The multi-degree-of-freedom mechanical arm drive controller of the present invention limits the parameters of stepper motor control to the displacement per unit time. The processor cycle control is used to refresh the control register to achieve dual control of speed and displacement. While not significantly increasing the workload of the processor, the design complexity of the programmable logic direction is greatly simplified.

[0037] The multi-degree-of-freedom mechanical arm drive controller of the present invention implements the protector in the hardware of the programmable logic part, thereby avoiding frequent software changes that introduce bugs and cause protection failure.

[0038] The multi-degree-of-freedom mechanical arm drive controller of the present invention uses programmable logic to perform secondary emergency stop protection on the protector, which can interrupt the output signal and realize emergency stop without power failure.

[0039] The present invention is applicable to any level of storage through the implementation of a truth table, and is more flexible in rewriting data, and avoids the problem of additional real-time overhead caused by the additional introduction of decoding.

[0040] Of course, any product implementing the present invention does not necessarily need to achieve all of the above advantages at the same time. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings required for describing the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other accompanying drawings can be obtained based on these accompanying drawings without paying creative work.

[0042] Figure 1 It is an operation flow chart of the multi-degree-of-freedom mechanical arm drive controller of the present invention;

[0043] Figure 2 It is a schematic diagram of the online parallel collision detection between the robot arm and the palletizing grid according to an embodiment of the present invention;

[0044] Figure 3 A schematic diagram of path discretization and uniform speed execution according to an embodiment of the present invention;

[0045] Figure 4 A schematic diagram of a trigonometric function solving acceleration module according to an embodiment of the present invention;

[0046] Figure 5 This is a logical structure diagram of the forward kinematics solution module of the robot arm A according to an embodiment of the present invention;

[0047] Figure 6A schematic diagram of solving the shortest distance between two line segments according to an embodiment of the present invention;

[0048] Figure 7 Schematic diagram of a connecting rod shortest distance calculation module according to an embodiment of the present invention;

[0049] Figure 8 Schematic diagram of a robot arm-robot arm collision determination module according to an embodiment of the present invention;

[0050] Fig. 9 Schematic diagram of a discrete static posture generation module according to an embodiment of the present invention. DETAILED DESCRIPTION

[0051] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0052] See also Figure 1 As shown, the present invention is a multi-degree-of-freedom mechanical arm drive controller, including a processor module and a motor drive module, the processor module includes an ARM processing module, a clock reset signal processing module and a bus arbitration module, the ARM processing module is electrically connected to the clock reset signal processing module, the clock reset signal processing module is electrically connected to the bus arbitration module, and the bus arbitration module is connected to the motor drive module;

[0053] The motor drive module includes a motor drive bus communication module, a signal line shunt and convergence related IP module, a robot pulse drive module 1, a robot pulse drive module 2 and an emergency stop protection module. The motor drive bus communication module is electrically connected to the signal shunt and convergence related IP module, and the signal shunt and convergence related IP module is electrically connected to the robot pulse drive module 1 and the robot pulse drive module 2 respectively. The robot pulse drive module 1 and the robot pulse drive module 2 are both electrically connected to the emergency stop protection module.

[0054] Among them, the ARM processing module is responsible for high-level abstract work such as the algorithm software part, and is connected to the bus arbitration module.

[0055] Among them, the bus arbitration module provides bus arbitration services for other accelerated peripheral IPs.

[0056] The clock reset signal processing module receives the input reset signal and clock signal, and generates a synchronous reset signal to reset the peripheral safely.

[0057] Among them, the robot pulse driving module 1 and the robot pulse driving module 2 are the robot pulse driving IPs, and the robot pulse driving module 1 and the robot pulse driving module 2 are each responsible for the driving pulse of a six-degree-of-freedom robotic arm.

[0058] Among them, the emergency stop protection module is responsible for receiving the emergency stop protection level signal from the external hardware circuit.

[0059] Among them, the signal line branching and converging related IP modules include bus communication IP, which is responsible for receiving control signals from the processor and converting the above control signals into control commands and sending them to other IPs.

[0060] Among them, the six-degree-of-freedom robotic arm has no less than six rotating joints driven by stepper motors.

[0061] In the present invention:

[0062] Robotic arm pulse drive digital logic design:

[0063] The six-degree-of-freedom mechanical arm of the present invention has built-in rotating joints driven by six or more stepper motors, and the driving module of each stepper motor receives a total of three signals, namely, motor power-on, driving direction, and driving pulse.

[0064] Pulse generator:

[0065] The system controls the movement speed of the robot arm and thus the movement of the joint by controlling the rotation angle of the stepper motor per unit time. Driven by the power circuit, the stepper motor will advance one step angle for each pulse received. The task of the pulse generator is to synchronize the control signal from the bus to the low-speed control clock domain in each control cycle of 10ms (adjustable according to the actual system) and generate pulses according to the control instructions.

[0066] The main logic unit of the pulse generator is a counter driven by a 100MHz clock (adjustable according to the system). Between the maximum and minimum speeds of the motor, the pulse generator maps the number of pulses that need to be sent in 10ms to the corresponding motor speeds. When the rising edge of the control clock arrives, the pulse generator will synchronize all control registers and output the synchronized motor direction control signal, power-on signal, and enable signal. At the same time, the pulse generator will start the counter and continuously reverse the pulse output after the count overflows.

[0067] The mapping between the number of pulses and the corresponding motor speed is completed by the processor. During the system construction phase, a speed-frequency division coefficient lookup table is generated based on the motor's extreme performance (accuracy, speed) and FPGA hardware performance. After the high-level algorithm is parsed into motor instructions, the table is looked up to obtain the control parameters that should be output and written into the pulse generator.

[0068] Model Protector:

[0069] The rotational travel of each joint of the robotic arm has a limit. In order to ensure the safety of personnel and equipment, each joint of the robotic arm is only allowed to work within a certain range. When the designed travel limit is reached, the FPGA will generate an alarm signal. When there is no alarm in the logic of each part, the output pulse generated by the pulse generator can be output to the motor. In the alarm or disabled state, even if the pulse generator generates a pulse, the pulse signal will not be transmitted to the driver of the robotic arm. The output model protector is a safety protection module with relatively simple logic. Programmable logic is chosen to be implemented to prevent frequent changes and introduce design defects. When the output model protector is integrated, its reset and upper and lower limit pulse count values ​​are set according to the joint parameters it controls. When the system is in a power-off, disabled state or the pulse count reaches the upper and lower limits, the pulse generator signal is shielded.

[0070] Signal output device:

[0071] The pulse signal output device is responsible for interacting with the power drive circuit. It needs to adjust the phase of the logic signal and cut the excess output according to different types of drive circuits.

[0072] Communication interaction between processor and programmable logic:

[0073] The processor can send motor control instructions to the programmable logic through extended instruction sets or bus communication.

[0074] In the present invention, the system processor (ARM) and the programmable logic (FPGA) exchange data through the AXI4-Lite bus interface, and the rotation speed and rotation direction of the motor are controlled by writing the signed frequency division coefficient to the register in each control cycle; the power-on and power-off and enable control of the motor are realized by writing the control logic register; the position pulse register of the output protector is read to realize the processor to obtain the number of motor feedback pulses, thereby calculating the current posture of the robot arm to avoid cumulative errors;

[0075] Logical protection:

[0076] In order to avoid the robot arm losing control due to failure of the Soc system, the present invention also designs an emergency stop protection digital logic based on digital logic. This part of the digital logic works independently, is not controlled by the processor, and only accepts the control level of the external remote control module. Its essence is multiple logical "ands" independent of the Soc. When the external control signal is cut off, the pulse signal output to the motor will also be frequency.

[0077] When an emergency occurs, all output pulses are shielded. This method enables the robot arm to stop without power failure at the digital logic level. (If the robot arm stops suddenly due to power failure, that is, the power supply is directly cut off, the robot arm will collapse directly, which may cause a secondary accident).

[0078] The present invention designs a multi-DOF robot drive controller based on the processor + FPGA architecture. Its main functional units are implemented in the FPGA, and with the assistance of the processor, it can complete the drive control of multiple stepper motors. The complex functions and advanced algorithms of the robot are implemented in the processor. Finally, the processor parses the motor command and sends it to the FPGA to generate a physical pulse signal, and finally the power drive circuit module drives the rotation of the motor.

[0079] In the system designed by the present invention, the basic peripheral interface and the basic drive control module of the robot arm are implemented in the FPGA in the form of digital logic. Compared with the hardware circuit built with discrete components, this design significantly reduces the size of the controller and enhances the stability of the system; compared with the driver chip solidified by ASIC, this solution has a short implementation cycle and low cost, can be flexibly configured according to engineering requirements, has hardware programmability, has higher flexibility, and is easy to upgrade and replace. When the number of robot motors that need to be controlled increases, the number of control IPs can be flexibly increased to achieve control of the system.

[0080] See also Figure 2-Figure 9 The multi-degree-of-freedom robot drive controller disclosed in the present invention adopts the following steps to perform motion planning of the multi-degree-of-freedom robot in a static environment:

[0081] Step S01: obtaining a large number of posture points in the configuration space of the robot arm by random sampling, which are random intermediate posture points;

[0082] Step S02: determining whether the random intermediate posture points are legal, and retaining the legal random intermediate posture points;

[0083] Step S03: traverse all posture points and connect random intermediate posture points that meet the requirements to form a path;

[0084] Step S04: Check the formed path to ensure that it will not collide with the static environment or the robot itself, and form a legal path set; since this is done offline during the system construction phase, there is no requirement for real-time performance, and any high-precision and long-time collision detection algorithm is used in a high-performance server;

[0085] Step S05: Execute motion planning offline in common working scenarios of the future system and count the usage frequency of all paths; since this is done offline, there is no requirement for real-time performance, and the collision detection therein adopts any high-precision and long-time collision detection algorithm on a high-performance server;

[0086] Step S06: Filter a specified number of paths from the legal path set for encoding the collision grid, giving priority to those with high frequency of use. The number of paths is determined by the system's requirements for success rate;

[0087] Step S07: Divide the space into a number of grids according to the coordinates of the coordinate system; use 4096 grids of the Cartesian coordinate system, and other coordinate systems may also be used according to the scene, including but not limited to the horizontal cross-section polar coordinate system and the spherical coordinate system;

[0088] Step S08: Execute each path separately in an offline environment, and record the grids touched during the execution of the path. Since this is done offline, there is no requirement for real-time performance. The collision detection can use any high-precision and long-time collision detection algorithm on a high-performance server.

[0089] Step S09: construct a CDU, and generate a truth table for each path according to the collision data obtained in the above steps; taking 1024 paths as an example, a truth table with 4096 inputs and 1024 outputs is generated, and the truth table is written into the system;

[0090] Step S10: Acquire sensor data to obtain an occupancy grid of an obstacle, wherein the method of acquiring data includes but is not limited to a point cloud sensor or an image sensor plus machine vision or a model method;

[0091] Step S11: taking the grid of obstacles as the input of the truth table and obtaining the output;

[0092] Step S12: Use a search algorithm based on graph theory, including but not limited to the A* algorithm or the Dijkstra algorithm, to search for a route from the starting posture point to the target posture point.

[0093] Preferably, S01-S09 are system building steps and are completed in an offline state.

[0094] Preferably, steps S10-S12 are system execution steps and are completed online.

[0095] Preferably, the coordinate system in step S07 is a Cartesian coordinate system.

[0096] Preferably, the grids in step S07 include three types, namely:

[0097] 1. Areas that the robot can reach and where obstacles will appear;

[0098] Second, the area where the robot arm can reach but obstacles will not appear;

[0099] 3. Obstacles will appear, but the robot arm cannot reach the area.

[0100] Among the above three types of areas, only one area needs to be encoded. When encoding, the coverage of the second and third areas should be reduced as much as possible, and the first area should be fully covered.

[0101] Preferably, in step S11, the result of the output truth table is two situations: the path collides with the current obstacle or the path is still safe.

[0102] Example 1 of motion planning for a multi-degree-of-freedom robot in a static environment:

[0103] The present invention takes a robotic arm with 6 degrees of freedom as an example, and each dimensional decibel represents the state of one joint.

[0104] In motion planning based on random landmarks, collision detection takes up most of the computing resources. Within a fixed collision detection time, the number of collision detections required will significantly affect the efficiency of planning. Under the "Lazy strategy", the algorithm will only perform collision detection on the selected path when searching for a path, rather than on all paths, which will reduce the number of collision detections.

[0105] Since the sampled intermediate posture points are randomly generated and have nothing to do with the sampling time, state, or number of times, the random posture points searched by the PRM algorithm are pre-sampled and generated during the system design phase. The same sampling set is used for each motion planning. Through offline sampling and screening, some posture points are actively eliminated, including points far away from other legal postures or inefficient posture points that exceed task requirements. By pre-selecting the random sampled posture points offline, the search efficiency of PRM real-time execution is improved.

[0106] Motion planning algorithm prmsort (P S ,P D )as follows:

[0107]

[0108]

[0109] Define the motion planning algorithm as prmsort(P S , P D ). The starting posture point of the plan is P S , the planned target posture point is P D .

[0110] First, connect P S and P D Presort (P S , P D )), determine from P S To P D The direct path is in the path collection library, and the path will not collide with the static pallet. If the path passes the pre-search at this stage, further dynamic path collision detection (Collideroute(P S , P D)) to detect whether there will be dynamic collisions between robot arms when executing this path. If the path is legal, the path point sequence [P S , P D ] indicates the line.

[0111] If the direct path between the starting point and the end point cannot be used, then a random intermediate posture point set library is continuously used to obtain an intermediate posture point (P M i+1 = Random()), and use the same method to determine P S With p M i+1 Is the path between them legal? If the path is legal, the motion planning algorithm prmsort (P M i+1 , P D ), try searching for P M i+1 , P D The path between them and generates the path [P S , P M i+1 ,…,P D ]. Otherwise, when the number of iterations is exhausted or the timeout condition (TIMEOUT) is triggered, the loop is exited and the search failure is returned.

[0112] In the path pre-search stage, the system needs to determine whether the two selected posture points can form a path and whether the formed path collides with the pallet.

[0113] Path pre-search algorithm Presort (E B , E E )as follows:

[0114]

[0115] Define the path pre-search algorithm as Presort (E B , E E ). The starting point of the robot path is E B ; The end point of the robot path is E E .

[0116] First, all paths in the path collection library are collided with the pallet (CollideGrid()), and safe paths are marked. Then, the path collection is searched for the path that connects E B , E E Route(E B , E E)). If found, it is determined that this path meets the requirements and will not collide with the pallet. Otherwise, this path may not be added to the path collection library because it does not meet the requirements or it collides with the pallet.

[0117] The random intermediate pose point set library used in random landmark search and the path set library used in path pre-search play a very important role in motion planning. By generating an index table, the path and pose points work together.

[0118] The generation of the collection library includes: offline generation of random intermediate posture points, offline generation and screening of paths. Although the tasks that the robot needs to complete are changing, the terminal positions of its starting posture points and target posture points are limited. By traversing all starting posture points and target posture points, the inverse kinematics of the robot posture is solved to obtain all the necessary starting posture points and target posture points.

[0119] The random intermediate posture point is a bridge connecting the starting posture point and the target posture point. In one motion planning, the robot arm will move from the starting posture point to several random intermediate posture points, and finally to the target posture point. The random intermediate posture points that are appropriately far away from the starting posture point and the target posture point can bypass obstacles and improve the success rate of the search. The robot arm can still find a path when it is too close to an obstacle.

[0120] In sampling, the end position of the robot arm is sampled first, and the end position and azimuth are constrained within the allowable range. Then the postures of the six joints of the robot arm are obtained by solving inverse kinematics, and illegal postures are eliminated to complete the generation of the random sampling intermediate posture point set library.

[0121] As shown, the collision detection principle of the robot arm palletizing is:

[0122] During the system construction phase, the space to be detected is divided into grids, and each grid is numbered and indexed. In the offline simulator, each path in the path collection library is executed in turn, and the grids that collide with the robot arm during the execution of the path are recorded. Each path is represented by the grid code where the collision occurs. The collision data is stored in the combinational logic circuit, which becomes a collision detection unit (CDU).

[0123] The upper-level system identifies the state of the palletizing and converts the palletizing into the corresponding occupied grid as the obstacle input of the static palletizing collision detection accelerator.

[0124] During real-time operation, the pallet occupancy grid coding is logically ANDed in parallel with the grid indices of all paths to be tested. When there is an overlap between the path occupancy grid and the pallet occupancy grid, a collision is considered to have occurred, and this path is an illegal path in the current motion planning. Since the real-time collision detection process is performed in parallel for all paths, the execution speed is extremely fast.

[0125] The space is divided into grids according to coordinates, and each grid is indexed according to its position in sequential coding. In the offline simulation environment, under the control of an automated script, the robotic arm sequentially executes each path that needs to be coded and records the grid indices that are collided with. Each path is recorded using the grid numbers where collisions occur to generate the CDU. For example, when executing the 10th path and colliding with grid 1 and grid 3, the CDU of path 10 is represented as:

[0126] Edge 10 = G1 | G3;

[0127] The online derivation method of the pallet occupancy grid is as follows:

[0128] The occupancy grid of the pallet on the logistics trolley is generated by the superior system: before emptying the goods on the logistics trolley, the stacked goods are continuously growing on the original basis. For the placed items, their sizes and positions are known, so the superior system can continuously accumulate the grids occupied by the current pallet during the sorting and stacking process and generate an index sequence of the grids according to the position of the current logistics trolley. If the pallet occupies grid 2 and grid 3 in the current state, the obstacle is represented as:

[0129]

[0130] As Figure 2 shown, when performing real-time static pallet collision detection, the obstacle grids input by the superior system are input in parallel to 1024 CDUs to determine whether the path will collide with the pallet. All paths with the CDU output of "0" are legal paths and are used to participate in the subsequent random roadmap search.

[0131] The principle of dynamic path collision detection is:

[0132] When one robot arm (hereinafter referred to as the planning robot arm) is performing motion planning and executing a path, the other robot arm (hereinafter referred to as the obstacle robot arm) is in dynamic motion, and its posture is changing almost every moment. Due to the differences in the tasks to be performed, the two robots cannot perform motion planning at the same time. After the planning is completed, this system will stamp the execution path of the planning robot arm with a start timestamp and an end timestamp. In the process of alternating roles between the two robots, the route of the robot arm that is planned and started earlier will have a higher priority and become an obstacle. The newly planned route must actively avoid collisions with the earlier planned route. At the same time, the posture of the obstacle robot arm at a certain moment can be accurately calculated based on the timestamp, and used for collision detection of discrete postures.

[0133] The dynamic continuous robot path is discretized into 32 static robot postures (hereinafter referred to as discrete static postures) for collision detection. With a certain step value, the postures of the planning robot arm and the obstacle robot arm at several future time points are calculated, and the discrete static postures are combined for static robot arm-robot arm collision detection. The safety loss caused by discrete detection is compensated by increasing the threshold of the safety distance. When all discrete postures are judged to be safe, the path is determined to be safe.

[0134] In order to avoid the inconsistency between the posture during the execution of the path and the posture during the collision detection process caused by the difference in the rotation speed of the six joints, it is agreed that when detecting and executing a path, all joints are started at the same time and reach the target position at the same time, that is, the entire path is completed at a uniform speed throughout the process.

[0135] In the collision detection of discrete postures, the improved bounding box method is used to regard the links of the two static robotic arms as line segments in space, and the coordinates of each link in Cartesian space are solved through the joint state of the robotic arms. The process of inductive collision detection is to calculate the shortest distance d between two line segments in space, and to determine whether d satisfies the constraints of the two link radii and safety thresholds, thereby determining whether the links of the two robotic arms collide.

[0136] When all the link pairs of the two robotic arms do not collide, it is considered that the two robotic arms will not collide in the current posture.

[0137] The dynamic path collision detection process algorithm is as follows:

[0138]

[0139]

[0140] Define the path collision detection algorithm as Collideroute(E B , E E ). The starting point of the planned robot path is EB , the end point of the planned robot path is E E .

[0141] 32 discrete static postures are obtained based on EB and EE calculations and the corresponding prediction timestamp Obtain the discrete static posture of the obstacle robot arm in the route it is executing at the same time based on the predicted timestamp

[0142] The 32 sets of discrete static postures are subjected to static manipulator-manipulator collision detection: first, the trigonometric functions (sinθ, cosθ) of each joint are solved in advance; second, the endpoint coordinates (LD) of each link are calculated according to the forward kinematics equation; finally, the shortest distance (d) of the 8 sets of links is obtained and it is determined whether this set of postures collides (PoseState).

[0143] After obtaining the collision status of 32 sets of discrete postures, determine whether the current path collides with the path that the obstacle robot is executing. Finally, return the collision detection result (RouteState).

[0144] The dynamic path collision detection accelerator is composed of multiple basic functional units. Some basic functional units will be implemented in FPGA in hardware form, using hardware parallelism and DSP (Digital Signal Processor) Slice to accelerate the execution speed of key operations, thereby accelerating dynamic path collision detection. In order to facilitate the exchange of data between software and hardware implementation, each basic functional unit mainly uses Q format to exchange data during design.

[0145] The design of basic functional units includes: trigonometric function solution module design, forward kinematics solution module design, connecting rod shortest distance calculation module design, robot-robot collision judgment module design, discrete static posture generation module design, and dynamic path collision judgment module design.

[0146] Trigonometric function solving module:

[0147] Before solving the forward kinematics, the input radians of each joint need to be converted into corresponding trigonometric function values ​​in advance. A more economical way to solve the trigonometric function is to save the sine table in RAM and use the table lookup method to obtain the result.

[0148] Each joint stroke of the robot arm has an upper and lower limit. By using the periodicity, parity, and phase difference of the trigonometric function, the sine and cosine function can be solved with very little RAM consumption. Figure 3 As shown in the following text, M 1 express.

[0149] M1 The hardware accelerator works under the control of the top-level module state machine and requires 7 clock cycles to complete basic functions:

[0150] In the first state (S 0 ), determine the input arc of each joint (θ i ) in the value range, by phase shifting θ i Align to RAM corresponding to sine and cosine (sθ i 、cθ i ) address; in the following S 1 , S 2 , S 3 , S 4 , S 5 The sine and cosine addresses corresponding to each joint are sequentially transferred to the dual-port RAM using a pipeline method, and then 2 , S 3 , S 4 , S 5 , S 6 The state reads the table lookup result; finally, according to θ i The sign of the correction result in the interval.

[0151] M 1 Each time, the radian values ​​of the five joints of a robot are input, using 16-bit, Q13 format, which can represent radians between -4rad and +3.9999rad, covering the travel of each joint of the robot. The output result of M1 is the sine and cosine values ​​of the corresponding joints, using 8-bit, Q7 format, which can represent trigonometric function values ​​ranging from -1 to +0.9921875.

[0152] Forward kinematics solver module:

[0153] The forward kinematics solution can map the robot from the joint space to the Cartesian space. Through the forward kinematics calculation, the coordinates of the five connecting rod nodes of the robot are obtained. The design principle of the forward kinematics solution module of robot A is as follows: Figure 4 As shown in the following text, M 2 express.

[0154] like Figure 4 As shown in the figure, the circle represents the calculation to be performed, and the text in the box represents the calculation result of the corresponding value, s represents sine, and c represents cosine. i X,LD i Y,LD i Z represents the Cartesian coordinate of the connecting rod end point to be solved.

[0155] M 2The hardware accelerator works under the control of the top-level module state machine and requires 4 clock cycles to complete basic functions:

[0156] Among them LD 0 , LD 1 The Cartesian coordinates of M are constants, and the results are produced directly. 2 According to the DH parameters of the six-DOF robot used by the system, the calculation process is simplified, the calculation order is adjusted, some pre-calculated results are shared, and the functional structure of the DSP Slice of the Artix-7 series FPGA is fully utilized to solve the LD in sequence without affecting the circuit timing. 2 , LD 3 , LD 4 coordinate.

[0157] M 2 The input is the 10 trigonometric function values ​​required to solve the forward kinematics of a robot arm, represented in 8-bit, Q7 format. 2 The output is the Cartesian coordinates of the five endpoints of the four links of the robot arm, expressed in 16-bit, Q3 format, which can represent Cartesian coordinates of -1024mm to +1024mm.

[0158] Connecting rod shortest distance calculation module:

[0159] Connecting rod shortest distance calculation module (M 3 ) The two connecting rods involved in the detection are regarded as two line segments in space (L s , L t ), where L s The starting point and end point are s 0 、s 1 , L t The starting point and end point are t 0 ,t 1 . Use the principles of spatial analytic geometry to find the shortest distance between two line segments. The solution process is as follows:

[0160] Define two lines l in space s , l t They are line segments L s (s 0 -s 1 )、L t (t 0 -t 1 ) on the straight line. The shortest distance points of the two line segments are s x ,t x The direction vectors are:

[0161]

[0162] Let the shortest distance between two points on the straight line be s j ,t j , their corresponding scalar is s c ,t c Then we have:

[0163]

[0164] make

[0165]

[0166] Then we have:

[0167]

[0168] When two lines are not coincident and parallel, The line where the two lines are located is the only common perpendicular line between the two lines, that is:

[0169]

[0170] Then we can obtain:

[0171]

[0172] make

[0173]

[0174] We can get:

[0175]

[0176] When ac-d2=0, the two straight lines are parallel. By taking any point on the straight lines as the common perpendicular line, the foot of the common perpendicular line on the other straight line can be found.

[0177] For the position of two line segments in space, the position of the shortest distance point needs to be discussed in 9 cases: c or c When s is less than 0, j or j Outside the starting point of the line segment, s 0 or 0 is the shortest distance point; when s c or c When s is greater than 1, j or j Outside the end point of the line segment, s 1 or 1 is the shortest distance point; when s c or c When the value of the foot of the common perpendicular line between the two straight lines is between 0 and 1, j ,t jThat is, the shortest distance point s between the two line segments x ,t x .

[0178] Computational models such as Figure 5 As shown,

[0179] The shortest distance between two line segments is: d(L s ,L t )=|s x -t x |.

[0180] Modules such as Figure 6 As shown, M 3 The hardware accelerator works under the control of the top-level module state machine and requires 13 clock cycles to complete basic functions:

[0181] In the first 6 cycles, M 3 According to the coordinates of the two connecting rods, vector calculation is performed, and the positional relationship of the shortest distance points of the two connecting rods is preliminarily determined. In the next three cycles, if the shortest distance point is on the line segment, M 3 Perform 6-bit precision division in parallel to calculate the position of the shortest distance point on the line segment. In the last 4 cycles, M 3 The specific coordinates of the two shortest distance points are calculated, and the square of the shortest distance is calculated for subsequent robot-robot collision judgment.

[0182] M 3 The input is the Cartesian coordinates of the four endpoints of the two connecting rods that need to be determined, expressed in 16-bit, Q3 format.

[0183] M 3 The output is the square of the shortest distance after reducing the precision and using 18-bit representation.

[0184] Robotic arm-robotic arm collision determination module:

[0185] After obtaining the shortest distances of the eight connecting rod pairs, it is possible to determine whether the connecting rods collide based on the pre-set collision detection threshold. Figure 7 As shown in the following text, M 4 express.

[0186] M 4 The hardware accelerator works under the control of the top-level module state machine and requires 1 clock cycle to complete basic functions:

[0187] After receiving the shortest distance of 8 sets of link pairs, M 4 According to the radius of the connecting rod and the safety threshold, each shortest distance is compared with the corresponding threshold. The comparison result is logically "OR" operated. Only when all the distances of the connecting rods are legal

[0188] (output logic 0), static robot-robot collision detection is legal (output logic 0).

[0189] M 4 The input is the square of the 8 shortest distances, represented by 18 bits. 4 The output is a 1-bit Boolean value indicating whether a collision occurred.

[0190] Discrete static pose generation module:

[0191] In the process of dynamic path collision detection, the discrete static posture generation module needs to generate 32 discrete static postures of the planned manipulators according to the path starting point and the path end point; according to the predicted timestamp, the posture of the obstacle manipulator at the corresponding time is calculated. Finally, the generated discrete static postures are combined to perform static manipulator-manipulator collision detection. The safety of the dynamic path is indirectly detected through collision detection between discrete static postures. The module principle is as follows Figure 8 As shown in the following text, M 5 express.

[0192] M 5 The hardware accelerator of M works in a pipeline-like manner according to the requirements under the control of the top-level module state machine. 5 Each clock cycle calculates a static discrete posture of the planned robot arm and generates a corresponding predicted timestamp. 5 The start and end timestamps of the route are used to calculate the corresponding time offset as the address of the lookup table, and a discrete static posture of the obstacle robot is read out in the next cycle for detection.

[0193] For subsequent testing, after completing motion planning, M 5 The start timestamp, end timestamp and each static discrete posture point of the final route after planning need to be written into the lookup table.

[0194] M 5 When working, you need to enter the path starting point E of the planned robot arm. B and the path end point E E The radian values ​​of the five joints are represented in 16-bit, Q13 format. At the same time, in order to predict the timestamp of each discrete static posture, the timestamp of the path starting point needs to be input in 32-bit format. 5 Each time, two sets of discrete static postures are output, represented in 16-bit, Q13 format.

[0195] M 5In the initialization state, you need to input no more than 700 discrete static postures, represented by 16-bit, Q13 format. At the same time, in order to calculate the search address of the obstacle robot, you also need to enter the start and end timestamps of the entire planned route, represented by 32-bit respectively.

[0196] Dynamic path collision determination module:

[0197] After obtaining 32 sets of static robot-robot collision detection results, if any set collides, the entire path will be considered to have collided by the dynamic path collision judgment module. Fig. 9 As shown in the following text, M 6 express.

[0198] M 6 The hardware accelerator works under the control of the top-level module state machine and requires 1 clock cycle to complete basic functions:

[0199] After all 32 sets of collision detection results are returned, M 6 Perform a logical "OR" operation on them, and the output result will be logical 0 (no collision) only when all the returned results are logical 0 (no collision).

[0200] M 6 The input is a 32-bit Boolean value indicating whether each posture has a collision. 6 The output is a 1-bit Boolean value indicating whether a collision occurs on the path.

[0201] In this embodiment, the six-degree-of-freedom robotic arm motion planning algorithm in the item sorting robot system is analyzed and designed; according to the system application scenario, the motion planning algorithm process is improved and designed based on the random landmark method, a path pre-search algorithm is designed, a static stacking collision detection accelerator and a dynamic path collision detection accelerator are designed.

[0202] In the design of the path pre-search algorithm, offline optimization is performed and a collection library is generated to improve planning efficiency. The collision detection of static palletizing is integrated into the path pre-search of motion planning, and static palletizing collision detection is completed while completing the path search.

[0203] In the design of the static palletizing collision detection accelerator, an improved space segmentation strategy is adopted: a large amount of computing work is completed offline using raster encoding paths and saved in the CDU; through parallel execution of real-time detection, it is quickly determined whether all paths are legal.

[0204] In the design of the dynamic path collision detection accelerator, an improved bounding box method is used: the static posture of the dynamically moving robot arm at each moment is calculated according to the timestamp; the static robot arm link model is simplified and regarded as a line segment in space; the shortest distance between two line segments in space is used to determine whether the two robots collide. The dynamic path collision detection accelerator consists of several basic functional units, including: trigonometric function solution module, forward kinematics solution module, link shortest distance calculation module, robot-robot collision judgment module, discrete static posture generation module, and dynamic path collision judgment module. The dynamic path collision detection accelerator mainly provides performance acceleration through hardware parallelism and DSP Slice; when detecting whether the path is safe, the "Lazy strategy" is adopted to only detect whether the selected path is legal.

[0205] In the description of this specification, the description with reference to the terms "one embodiment", "example", "specific example", etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or more embodiments or examples in a suitable manner.

[0206] The preferred embodiments of the present invention disclosed above are only used to help explain the present invention. The preferred embodiments do not describe all the details in detail, nor do they limit the invention to only specific implementation methods. Obviously, many modifications and changes can be made according to the content of this specification. This specification selects and specifically describes these embodiments in order to better explain the principles and practical applications of the present invention, so that those skilled in the art can understand and use the present invention well. The present invention is limited only by the claims and their full scope and equivalents.

Claims

1. A multi-degree-of-freedom robotic arm drive controller, comprising a processor module and a motor drive module, characterized in that: The processor module includes an ARM processing module, a clock reset signal processing module and a bus arbitration module, wherein the ARM processing module is electrically connected to the clock reset signal processing module, the clock reset signal processing module is electrically connected to the bus arbitration module, and the bus arbitration module is connected to the motor drive module; The motor drive module includes a motor drive bus communication module, a signal line shunt and confluence related IP module, a first robot pulse drive module, a second robot pulse drive module and an emergency stop protection module, the motor drive bus communication module is electrically connected to the signal shunt and confluence related IP module, the signal shunt and confluence related IP module is electrically connected to the first robot pulse drive module and the second robot pulse drive module respectively, and the first robot pulse drive module and the second robot pulse drive module are both electrically connected to the emergency stop protection module; The multi-DOF robot drive controller uses the following steps to perform multi-DOF robot motion planning in static environments: Step S01: obtaining a large number of posture points in the configuration space of the robot arm by random sampling, which are random intermediate posture points; Step S02: determining whether the random intermediate posture points are legal, and retaining the legal random intermediate posture points; Step S03: traverse all posture points and connect random intermediate posture points that meet the requirements to form a path; Step S04: Check the formed path to ensure that it will not collide with the static environment or the robot itself, and form a legal path set; Step S05: Execute motion planning offline in common working scenarios of future systems and count the usage frequencies of all paths; Step S06: Filter a specified number of paths from the legal path set for encoding the collision grid, giving priority to those with high frequency of use. The number of paths is determined by the system's requirements for success rate; Step S07: dividing the space into a number of grids according to the coordinates of the coordinate system; Step S08: executing each path separately in an offline environment, and recording the grids touched during the execution of the path; Step S09: construct CDU, and generate a truth table for each path according to the collision data obtained in the above steps; Step S10: Acquire sensor data to obtain an occupancy grid of an obstacle; Step S11: taking the grid of obstacles as the input of the truth table and obtaining the output; Step S12: Use a graph theory-based search algorithm to search for a route from the starting posture point to the target posture point.

2. The multi-degree-of-freedom robot arm drive controller according to claim 1, characterized in that: The ARM processing module is responsible for high-level abstract work such as the algorithm software part, and is connected to the bus arbitration module.

3. The multi-degree-of-freedom robot arm drive controller according to claim 1, characterized in that: The bus arbitration module provides bus arbitration services for other acceleration peripheral IPs.

4. The multi-degree-of-freedom robot arm drive controller according to claim 1, characterized in that: The clock reset signal processing module receives the input reset signal and clock signal, and generates a synchronous reset signal to reset the peripheral safely.

5. The multi-degree-of-freedom robot arm drive controller according to claim 1, characterized in that: The first robot pulse driving module and the second robot pulse driving module are robot pulse driving IPs, and the first robot pulse driving module and the second robot pulse driving module are each responsible for the driving pulse of a six-degree-of-freedom robotic arm.

6. The multi-degree-of-freedom robot arm drive controller according to claim 1, characterized in that: The emergency stop protection module is responsible for receiving the emergency stop protection level signal from the external hardware circuit.

7. The multi-degree-of-freedom robot arm drive controller according to claim 1, characterized in that: The signal line shunting and converging related IP modules include a bus communication IP, which is responsible for receiving control signals from a processor and converting the control signals into control commands and sending them to other IPs.

8. The multi-degree-of-freedom robot arm drive controller according to claim 5, characterized in that: The six-degree-of-freedom robotic arm has built-in rotating joints driven by no less than six stepper motors.

9. The multi-degree-of-freedom robot arm drive controller according to claim 1, characterized in that: The steps S01-S09 are system building steps and are completed in an offline state; Steps S10-S12 are system execution steps and are completed online; The grids in step S07 include three types, namely:

1. Areas that the robot can reach and where obstacles will appear; Second, the area where the robot arm can reach but obstacles will not appear; 3. Obstacles will appear, but the robot arm cannot reach the area; In step S11, the truth table outputted has two results: the path collides with the current obstacle or the path is still safe.

Images