Mechanical arm control system and control method

Abstract

The invention relates to a mechanical arm control system and method, and belongs to the field of mechanical arm control, and the control system comprises a microkernel controller which calculates the joint angle of each joint of a mechanical arm according to the difference value between the current position and the target position of the tail end of the mechanical arm and sends the joint angle to a servo driver; the servo driver is responsible for closed-loop control of a current loop and a speed loop, accurately executes the position instruction sent by the microkernel controller and feeds back the actual state of each joint to the microkernel controller in real time; the multi-degree-of-freedom mechanical arm serves as an executing mechanism, receives the joint movement instruction from the servo driver, and drives an end effector to achieve any specified pose and complex track in a three-dimensional space through cooperative movement of a plurality of joints; and the instruction unit is used for communicating with the microkernel controller and issuing the target position information of the end effector of the multi-degree-of-freedom mechanical arm to the microkernel controller. By adopting the control method of the control system, the control precision and reliability of the mechanical arm can be improved.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of mechanical arm control, and particularly relates to a mechanical arm control system and a control method. BACKGROUND

[0002] The existing mechanical arm control system mostly adopts a general operating system (GPOS) or a real-time operating system (RTOS). The GPOS system is large, is interfered by non-real-time tasks, and is difficult to meet the high-precision control requirement; the RTOS is good in real-time performance, but is high in module coupling degree and difficult in function expansion. The micro-kernel architecture has high real-time performance and high reliability through minimization of core functions and service isolation, but the prior art does not combine the micro-kernel architecture with mechanical arm control. The application realizes deterministic response and function safety of mechanical arm control by applying the improved embedded micro-kernel operating system controller. SUMMARY

[0003] The application aims to provide a mechanical arm control system and a control method, which realize task scheduling determinacy, system service isolation and fault self-recovery through the micro-kernel architecture, and improve the precision and reliability of mechanical arm control.

[0004] In order to achieve the above-mentioned purpose, the first technical solution adopted by the application is a mechanical arm control system, comprising: A micro-kernel controller, which carries an embedded micro-kernel operating system, is responsible for calculating joint angles of joint modules of a mechanical arm according to a difference between a current position and a target position of a terminal of the mechanical arm and sending the joint angles to a servo driver through an EtherCat Ethernet bus; The servo driver is responsible for current loop and speed loop closed-loop control, accurately executes a position instruction issued by the micro-kernel controller, and feeds back actual states of joints to the micro-kernel controller in real time; A multi-degree-of-freedom mechanical arm, as an execution mechanism, receives joint motion instructions from the servo driver, and drives an end effector to realize any specified pose and complex trajectory in a three-dimensional space through cooperative motion of multiple joints; An instruction unit is used for communication with the micro-kernel controller and issues target position information of an end effector of the multi-degree-of-freedom mechanical arm to the micro-kernel controller; The micro-kernel operating system is divided into a micro-kernel layer, a system service layer and an application layer from bottom to top; the micro-kernel layer is used for task scheduling, memory management, interrupt processing and IPC communication; the system service layer includes EtherCAT drive services, file system services and safety monitoring services, the system service layer runs in a user state, communicates with the micro-kernel layer through IPC, and there is no direct interaction between services; the application layer runs in the form of an independent task, acquires resources through task scheduling of the micro-kernel layer, and includes kinematics modeling tasks, trajectory planning tasks, inverse kinematics analysis tasks and closed-loop correction tasks.

[0005] Further, the task scheduling of the microkernel layer adopts a priority preemption algorithm, and a task switching time is less than 5 mu s.

[0006] Further, the memory management adopts a static memory allocation mechanism, and fixed memory spaces are allocated to the kernel mode tasks and the user mode services.

[0007] Further, the interrupt processing focuses on critical interrupts, so as to shorten an interrupt response delay, and the critical interrupts include EtherCAT bus interrupts and joint position feedback interrupts.

[0008] Further, the IPC communication adopts a message passing mechanism to realize safe communication between the kernel mode and the user mode and between the user mode services.

[0009] Further, in the system service layer: (1) the EtherCAT drive service is responsible for EtherCAT bus protocol analysis and data transceiving, processes communication logic between the microkernel controller and the servo driver, and does not participate in joint control; (2) the file system service manages D-H parameter files and trajectory data files in the form of providing read-write interfaces and not participating in kinematics modeling or trajectory planning; and (3) the safety monitoring service is used for monitoring processor temperature, memory usage and communication state in real time, and triggering a protection mechanism when detecting an exception.

[0010] Further, in the application layer: (1) the kinematics modeling task establishes a forward kinematics model of the mechanical arm based on a D-H parameter method; (2) in the trajectory planning task, a target pose of an end is received, a smooth path point sequence is generated by adopting a quintic polynomial interpolation, end motion velocity and acceleration are ensured to be continuous, and collision detection and path optimization are performed; (3) the inverse kinematics analysis task solves joint angle candidate solutions by adopting an analytical method, and filters optimal joint angles in combination with a shortest path principle and joint limit constraints; and (4) in the closed loop correction task, actual joint position data uploaded by the servo driver is received, an error between an actual end pose and a target end pose is calculated, joint angle instructions are adjusted by an adaptive PID algorithm, and trajectory tracking accuracy is ensured.

[0011] The application further provides a second technical scheme, and a mechanical arm control method. (1) system initialization: D-H parameters are loaded, a forward kinematics model is established, and self-checking of hardware and microkernel layer services is completed; (2) trajectory planning: a microkernel controller generates a smooth path of end effector motion by adopting a quintic polynomial difference algorithm based on a target pose and a current pose of a multi-degree-of-freedom mechanical arm end effector; (3) Inverse kinematics analysis: in each cycle, the candidate solutions of each joint angle of the multi-degree-of-freedom robot arm are calculated by an analytical method, and the optimal joint angle combination is selected according to the shortest path principle, joint limit and built-in real-time collision detection model; (4) Joint space real-time control: the microkernel controller sends the optimal joint angle command obtained by inverse kinematics analysis to the corresponding servo driver of each joint according to the cycle, and the servo driver controls the joint motor of the multi-degree-of-freedom robot arm through the position loop, speed loop and current loop, while the servo driver feeds back the actual angle, speed and current data of the joint to the microkernel controller, and the microkernel controller stores the feedback data to the closed-loop buffer area for subsequent correction. (5) Closed-loop correction: the microkernel controller reads the actual angle of the joint from the closed-loop buffer area, calculates the end effector pose error, and adjusts the joint angle command of the next cycle online through the adaptive PID algorithm to ensure that the error of the next cycle is less than the threshold.

[0012] The beneficial effects of the present application are: the microkernel operating system carried by the microkernel controller in the present application adopts a layered architecture, the microkernel only provides task scheduling, memory management, interrupt handling and IPC communication, the system services run in the user state, and module isolation is realized through message passing. Through the deterministic scheduling of the microkernel operating system and the isolation of the system services, the present application realizes microsecond-level response and high reliability of the robot arm control, and is suitable for high-end equipment fields such as precision assembly and surgical robots.

[0013] The real-time task response of the microkernel operating system described in the present application is ≤3μs, the context switching is ≤0.7μs, the interrupt delay is ≤2μs, the timing accuracy is ≤0.04μs, and the minimum cycle delay is ≤4μs, which effectively improves the real-time performance, reliability and control accuracy of the robot arm control. BRIEF DESCRIPTION OF DRAWINGS

[0014] Figure 1 The figure is the overall architecture diagram of the robot arm control system described in the present application.

[0015] Figure 2 The figure is the function division architecture diagram of the embedded microkernel operating system described in the present application.

[0016] Figure 3 The figure is the flow chart of the robot arm control method described in the present application.

[0017] Figure 4 The figure is the inverse kinematics analysis schematic diagram in the embodiment of the present application. DETAILED DESCRIPTION

[0018] The present application will be further described in detail below in combination with the drawings and embodiments, but it is not as the basis for any limitation on the invention.

[0019] The application provides a mechanical arm control system and a control method.

[0020] Embodiment 1 The mechanical arm control system according to the application has the overall architecture as shown in Figure 1 The system comprises: (1) a microkernel controller: carrying an optimized microkernel operating system, used for kinematics modeling, trajectory planning and inverse kinematics analysis, calculating joint angles of each joint module of the mechanical arm according to a difference between a current position and a target position of the mechanical arm end and sending the joint angles to a servo driver through an EtherCat Ethernet bus.

[0021] (2) a servo driver: as a bottom-layer driving execution unit, responsible for current loop and speed loop closed-loop control, accurately executing a position instruction from the controller and feeding back actual states of each joint in real time. The microkernel controller and the servo driver constitute a master-slave closed-loop system, fully exerting the intelligence of the microkernel controller in the whole machine planning level and the rapidity of the servo driver in the local control level, and jointly ensuring trajectory accuracy and dynamic response performance of the mechanical arm end.

[0022] (3) a multi-degree-of-freedom mechanical arm: as a core execution mechanism, the multi-degree-of-freedom mechanical arm adopts a serial structure mechanical arm, and its function is to receive high-precision joint motion instructions from the bottom-layer servo driver, drive the mechanical arm end effector to realize any specified pose and complex trajectory in a three-dimensional space through cooperative motion of multiple joints.

[0023] (4) an instruction unit: communicating with the microkernel controller, sending target position information of the mechanical arm end effector to the microkernel controller through Modbus / TCP.

[0024] The architecture of the optimized microkernel operating system in the microkernel control is as shown in Figure 2 The system comprises a three-layer structure from bottom to top: a microkernel layer (kernel state), a system service layer (user state) and an application layer (user state).

[0025] (1) the microkernel layer: providing task scheduling, memory management, interrupt processing and IPC communication, with a code size less than 10 KB; specifically comprising: (1-1) Task scheduling: 5-level priority is designed for the control task of the robot arm, and the tasks are strictly arranged according to the "control real-time": inverse kinematics analysis task (priority 5) > EtherCAT instruction issuing task (priority 4) > closed-loop correction task (priority 3) > trajectory planning task (priority 2) > non-real-time task (such as data logging, priority 1). High-priority tasks can instantly preempt low-priority tasks, and the task switching time is less than 5μs, ensuring that joint commands can be generated and issued on time within each control cycle.

[0026] (1-2) Memory management: Static memory allocation mechanism is adopted to avoid memory fragmentation and delay caused by dynamic allocation. The memory allocation certainty is less than 1μs, and only fixed memory space is allocated for kernel tasks and user service; (1-3) Interrupt handling: Focus on key interrupts (EtherCAT bus interrupt, joint position feedback interrupt), interrupt response delay <1μs, no redundant interrupt handling logic; (1-4) IPC communication (inter-process communication): Safe communication between kernel and user state and user service is realized through message passing mechanism, only data transmission interface is provided, and business logic processing is not involved; (2) System service layer: All non-core functions are encapsulated as independent processes running in user mode, communicating with the microkernel layer through IPC, and there is no direct interaction between services. Specifically, it includes: (2-1) EtherCAT driver service: responsible for EtherCAT bus protocol analysis, data transmission (supports Coe protocol), only handles "controller-servo driver" communication logic, does not participate in joint control; (2-2) File system service: manages D-H parameter files, trajectory data files, and only provides "read / write" interface, does not involve kinematics modeling or trajectory planning; (2-3) Safety monitoring service: real-time monitoring of processor temperature, memory usage, communication status, detecting abnormal triggering protection (such as emergency stop), not involved in normal control process; (3) Application layer: carries the core business logic of robot arm control, runs in the form of independent tasks, and acquires resources through the task scheduling of the microkernel, specifically including 4 types of core tasks: (3-1) Kinematics modeling task: establishes the forward kinematics model of the robot arm based on the D-H parameter method; (3-2) Trajectory planning task: receives the target pose of the robot arm end effector issued by the command unit, generates a smooth path point sequence using a quintic polynomial interpolation, ensures the continuity of the robot arm end effector motion speed and acceleration, and performs collision detection and path optimization;

[0027] (3-3) Inverse kinematics analysis task: adopt analytical method to solve joint angle candidate solution, combine shortest path principle, joint limit constraint and built-in real-time collision detection model to screen optimal joint angle combination; (3-4) Closed-loop correction task: receive joint actual position data uploaded by servo driver, calculate error between mechanical arm end effector actual pose and target pose, adjust joint angle instruction through adaptive PID algorithm, and ensure trajectory tracking accuracy.

[0027] Embodiment 2 The mechanical arm control method provided by the application adopts the control system described in Embodiment 1, and the method flow is as shown in Figure 3 , and the specific method is as follows: 1. System initialization and modeling (1) After the controller is powered on, initialize the processor, EtherCAT master station and Modbus / TCP interface, detect the connection state of the servo driver and the joints of the mechanical arm, automatically detect the connected mechanical arm configuration (such as six degrees of freedom, SCARA, etc.), and load the corresponding D-H parameters from the non-volatile memory; (2) Use D-H parameter method to establish the kinematics model of the mechanical arm: assign a link coordinate system to each joint i, and determine four parameters: link length (a i ) (along X axis), link torsion angle (α i ) (around X axis), joint offset (d i ) (along Z axis), and joint angle (θ i ) (around Z axis); (3) Calculate the homogeneous transformation matrix A i between adjacent link coordinate systems: ; (4) Get the pose matrix of the mechanical arm end effector relative to the base coordinate system {0} through chain multiplication: , which describes the position and attitude of the mechanical arm end effector.

[0028] 2. Task receiving and trajectory planning (1) The instruction unit sends the target pose T target of the mechanical arm end effector specified by the user to the microkernel controller. The target pose is expressed in coordinates and Euler angles in Cartesian space.

[0029] (2) Under the real-time task scheduling of the microkernel, the microkernel controller calculates the target pose T target according to the current mechanical arm end effector pose T current and the target pose T target, trajectory planning is performed. The planning algorithm uses quintic polynomial interpolation to generate a series of smooth intermediate path points in Cartesian space and calculates the desired pose of the end effector T desired (k).

[0030] 3. Real-time inverse kinematics analysis For each calculated desired pose T desired (k) of the control cycle, the microkernel controller executes an inverse kinematics analysis algorithm to calculate candidate solutions for the joint angles of the multi-degree-of-freedom robot arm, and selects the optimal joint angle combination according to the shortest path principle, joint limits, and an embedded real-time collision detection model , where θ i is the joint angle of the robot arm.

[0031] As shown in Figure 4 , taking a common 6-DOF robot arm PUMA configuration as an example: first, the base joint angle θ1 is solved by matrix operation, and then the shoulder joint angle θ2 and the elbow joint angle θ3 are solved in turn, and then the wrist joint angles θ4, θ5, and θ6 are solved using the geometric relationship of the wrist joint. At this time, in the joint angle combination, n = 6.

[0032] Thanks to the deterministic task scheduling and computational optimization of the microkernel, the entire inverse solution process can be completed within microseconds.

[0033] 4. Real-time joint space control (1) The microkernel controller sends the optimal joint angle command obtained by inverse kinematics analysis to the corresponding servo driver of each joint through the EtherCAT bus at regular intervals; (2) Servo driver control The servo driver controls the joint motor of the multi-degree-of-freedom robot arm through position loop, velocity loop, and current loop control; Position loop: receives the position command (joint angle) sent by the microkernel controller, compares it with the actual position feedback from the motor encoder, and generates a speed command; Velocity loop: receives the output of the position loop or the direct speed command from the microkernel controller, compares it with the actual speed feedback from the encoder, and generates a current (torque) command; Current loop: receives the output of the velocity loop and controls the motor torque through an IGBT inverter. The current feedback comes from a Hall sensor, and the current loop has the highest bandwidth (typically > 1 kHz), which is the key to achieving high dynamic response; (3) Servo driver feedback The servo driver packs and feeds data such as actual angle, speed, current, temperature and error code of the joint to the microkernel controller, and the microkernel controller stores the feedback data to the closed loop buffer area for subsequent correction.

[0034] 5. Closed loop feedback and online correction (1) The microkernel controller receives full closed loop feedback information from the servo driver (including actual joint angle and actual pose of the end effector); (2) Compare the actual pose with the desired pose, and calculate the pose error E(k) according to the following formula: ; (3) The controller uses the error information to fine-tune the joint angle command for the next time by the PID adaptive control algorithm, to compensate for model errors, friction, inertia and other disturbances, and finally realize high-precision trajectory tracking of the end effector of the robot arm.

[0035] The following takes a six-degree-of-freedom industrial robot arm control as an example to further illustrate the present application.

[0036] I. System hardware configuration 1. Microkernel controller: Processor: ARM Cortex-A53, 1.2GHz; Memory: 32GB, LPDDR4; Storage: 128GB, eMMC; Interface: 2-way EtherCAT master interface (industrial Ethernet port), 1-way standard Ethernet interface, supporting Modbus / TCP protocol; Operating system: embedded microkernel controller.

[0037] 2. Servo driver Six servo drivers are set up, using hybrid force position control mode, communicating with EtherCAT CoE protocol, and the position accuracy is ±0.001°.

[0038] 3. Robot arm A six-joint series structure is adopted, joints 1-3 are shoulder-elbow, joints 4-6 are wrist, the repeatability is ±0.02mm, the working radius of the robot arm is 1.5m, and the load capacity is 10kg.

[0039] 4. Instruction unit A 10-inch touch screen is used to interact with the microkernel controller through Modbus / TCP (100Mbps).

[0040] II. Control method 1. System initialization and modeling (1) Hardware initialization: The robot system is powered on, and the microkernel controller, servo driver, touch screen, joint motor, etc. are powered on and self-checked. If they are all normal, the status display light is green.

[0041] (2) Software initialization: Load the microkernel operating system, start the microkernel layer core service (task scheduling, memory management), initialize the system service layer (EtherCAT driver, file system, safety monitoring) and application layer task (kinematics modeling, etc.), and send the "ready signal" to the instruction unit after self-checking without exception.

[0042] (3) Kinematics modeling: Read the D-H parameter file from eMMC flash memory, assign link coordinate systems to the six joints, calculate the adjacent link homogeneous transformation matrix A1~A6, establish the mapping relationship between the end pose and the joint angle through chain multiplication (T=A1×A2×…×A6), generate the forward kinematics model, and store it in the real-time memory area.

[0043] 2. Task receiving and trajectory planning (1) Instruction receiving: The operator inputs the target pose of the robot end effector (X=800mm, Y=0mm, Z=500mm, attitude a=0°, b=0°, g=0°) through the touch screen. The touch screen sends the target pose to the microkernel controller through ModbusT / TCP.

[0044] (2) Trajectory generation: After the microkernel controller receives the target pose, it compares it with the current pose. The trajectory planning algorithm uses a quintic polynomial difference algorithm, sets the total motion time to 1s, the control period to 1ms, generates 1000 path points (each path point contains X t , Y t , Z t , a t , b t , g t ), and ensures the continuity of adjacent path points' speed and acceleration. At the same time, collision detection is performed - the robot link model (simplified as a cuboid) is compared with the preset tooling table model to confirm that there is no collision risk.

[0045] 3. Real-time inverse kinematics analysis (1) Path point traversal: The microkernel controller reads the path points from the trajectory cache area (a memory area in the robot controller used to temporarily store preprocessed motion trajectory points) at a 1ms period, triggering the inverse kinematics analysis task (priority 5, preempting other low-priority tasks).

[0046] (2) Analysis method solution: 1) Extract the target position of the wrist center point W; 2) The target position of W is given, and the closed solution of θ1~θ3 is derived by using "plane projection + cosine theorem" combined with the geometry of shoulder and elbow; 3) The joint angles θ4~θ6 of the wrist are decoupled and derived by using "rotation matrix series relationship"; 4) The microkernel controller selects the optimal joint angle combination from all feasible solutions according to the "shortest path principle" (the solution with the smallest sum of absolute values of the difference between each group of solutions and the "current joint angle" of the robot arm), the joint limit of the robot arm, and the built-in real-time collision detection model.

[0047] 4, Real-time joint space control (1) The microkernel controller sends the optimal joint angle command to the 6 servo drives through the EtherCAT bus every period. Each servo drive receives the target angle of the corresponding joint, and the bus transmission delay is < 50μs; (2) Servo three-loop control: control the mechanical arm joint motor action through position loop, speed loop and current loop; (3) State feedback: the servo drive feeds back the actual position, speed, current, temperature and other data of the joint to the microkernel controller every 1ms, and the microkernel controller stores the feedback data in the closed-loop buffer area for subsequent correction.

[0048] 5, Closed-loop feedback and online correction (1) Error calculation: the microkernel controller reads the actual angles of the 6 joints from the closed-loop buffer area, and calculates the actual pose of the end through the forward kinematics model. The pose error is obtained by comparing the actual pose with the target pose; (2) Adaptive PID correction: if the position error is less than the threshold 0.05mm and the attitude error is less than 0.05°, the current PID parameters remain unchanged; if the error of a certain period exceeds the threshold, the control algorithm will increase Kp and Ki to increase the correction strength, so that the error of the next period is less than the threshold.

[0049] The above examples are only used to illustrate the technical solutions of the present application and not to limit them. Those skilled in the art should understand that the specific embodiments of the present application can be modified or replaced by equivalents according to the above examples, and any modification or equivalent replacement that does not deviate from the spirit and scope of the present application is within the protection scope of the claims.

Claims

1. A robot control system, characterized by, Comprise: A microkernel controller, which is equipped with an embedded microkernel operating system, is responsible for calculating the joint angles of each joint module of the robot arm according to the difference between the current position and the target position of the end of the robot arm and sending the joint angles to the servo driver through an EtherCat Ethernet bus; A servo driver, which is responsible for current loop and speed loop closed-loop control, accurately executes the position instructions issued by the microkernel controller and feeds back the actual state of each joint to the microkernel controller in real time; A multi-degree-of-freedom robot arm, as an actuator, receives joint motion instructions from the servo driver and drives the end effector to achieve any specified pose and complex trajectory in three-dimensional space through the coordinated motion of multiple joints; An instruction unit is used to communicate with the microkernel controller and issue target position information of the end effector of the multi-degree-of-freedom robot arm to the microkernel controller; The microkernel operating system is divided into a microkernel layer, a system service layer and an application layer from bottom to top; the microkernel layer is used for task scheduling, memory management, interrupt processing and IPC communication; the system service layer includes EtherCAT drive services, file system services and security monitoring services, the system service layer runs in the user state and communicates with the microkernel layer through IPC, and there is no direct interaction between services; the application layer runs in the form of an independent task and obtains resources through task scheduling of the microkernel layer, including kinematics modeling tasks, trajectory planning tasks, inverse kinematics analysis tasks and closed-loop correction tasks.

2. The mechanical arm control system of claim 1, wherein, The task scheduling of the microkernel layer adopts a priority preemptive algorithm, and the task switching time is less than 5μs.

3. The robotic arm control system of claim 1, wherein, The memory management adopts a static memory allocation mechanism, and fixed memory space is allocated to kernel tasks and user services.

4. The robotic arm control system of claim 1, wherein, The interrupt processing focuses on key interrupts to shorten the interrupt response delay, and the key interrupts include EtherCAT bus interrupts and joint position feedback interrupts.

5. The robotic arm control system of claim 1, wherein, The IPC communication adopts a message passing mechanism to realize safe communication between the kernel state and the user state and user services.

6. The robotic arm control system of claim 1, wherein, In the system service layer: (1) the EtherCAT drive service is responsible for EtherCAT bus protocol analysis and data transmission and reception, handles the communication logic between the microkernel controller and the servo driver, and does not participate in joint control; (2) the file system service manages D-H parameter files and trajectory data files in the form of providing read-write interfaces without participating in kinematics modeling or trajectory planning; (3) the security monitoring service is used to monitor the processor temperature, memory usage and communication state in real time and trigger the protection mechanism when an exception is detected.

7. The robotic arm control system of claim 1, wherein, In the application layer: (1) the kinematics modeling task is based on D-H parameter method to establish the forward kinematics model of the robot arm; (2) in the trajectory planning task, the end target pose issued by the receiving instruction unit is adopted to generate a smooth path point sequence by using a quintic polynomial interpolation, to ensure the continuity of the end motion velocity and acceleration, and to perform collision detection and path optimization; (3) the inverse kinematics analysis task adopts an analytical method to solve the joint angle candidate solution, and combines the shortest path principle and joint limit constraint to screen the optimal joint angle; (4) in the closed-loop correction task, the actual joint position data uploaded by the servo driver is received, the error between the actual end pose and the target pose is calculated, the joint angle command is adjusted through an adaptive PID algorithm, and the trajectory tracking accuracy is ensured.

8. A method of controlling a robot arm, the method being performed using a robot arm control system according to any one of claims 1 to 7, characterized by The method comprises the following steps: (1) system initialization: loading D-H parameters, establishing a forward kinematics model, and completing self-checking of hardware and microkernel layer services; (2) trajectory planning: the microkernel controller generates a smooth path of the end effector motion based on the target pose and the current pose of the end effector of the multi-degree-of-freedom robot arm by using a quintic polynomial difference algorithm; (3) inverse kinematics analysis: in each cycle, the analytical method is used to solve the joint angle candidate solution of the multi-degree-of-freedom robot arm, and the optimal joint angle combination is screened according to the shortest path principle, joint limit and built-in real-time collision detection model; (4) joint space real-time control: the microkernel controller issues the optimal joint angle command obtained by the inverse kinematics analysis to the servo driver corresponding to each joint according to the cycle, the servo driver controls the joint motor of the multi-degree-of-freedom robot arm through the position loop, velocity loop and current loop, and simultaneously feeds back the actual angle, velocity and current data of the joint to the microkernel controller, and the microkernel controller stores the feedback data in the closed-loop buffer area for subsequent correction; (5) closed-loop correction: the microkernel controller reads the actual angle of the joint from the closed-loop buffer area, calculates the end effector pose error, and adjusts the joint angle command of the next cycle online through an adaptive PID algorithm, to ensure that the error of the next cycle is less than a threshold.

Images