System and method for real-time kinematic forward and inverse solution computation for robots

Abstract

The application discloses a kind of system and method for robot real-time kinematics forward and inverse solution calculation, it is related to robot control technical field, system includes: containing data interface module, positive solution calculation module, inverse solution lookup table module and error compensation module.Data interface module receives target pose or each joint's preset angle sent by main control unit.Positive solution calculation module converts each joint's preset angle into end effector pose by hardware parallel computation.Inverse solution lookup table module converts target pose into each joint's initial angle by lookup table method.Error compensation module carries out error compensation and singularity detection to the initial angle of each joint, obtains the target angle of each joint.Data interface module feeds back the calculation result to main control unit, to control robot movement.The application is combined by hardware parallel computation and lookup table method, improves calculation speed and real-time, to realize high-precision control with lower cost and power consumption.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, and in particular to a system and method for real-time forward and inverse kinematics calculation of a robot. Background Technology

[0002] In robot control systems, real-time and high-precision calculation of forward and inverse kinematics is the core foundation for achieving precise robot motion control. Forward kinematics refers to the mapping from joint space to manipulator space, that is, calculating the pose (including position and orientation) of the robot's end effector based on the angles of each joint. Inverse kinematics, on the other hand, is the mapping from manipulator space to joint space, that is, solving for the required angles of each joint based on the desired end effector pose. For multi-degree-of-freedom robots (such as 6 axes and above), their kinematic equations, especially the inverse equations, involve solving a large number of trigonometric functions and nonlinear equations, resulting in complex and computationally intensive calculations.

[0003] Currently, existing technologies for achieving the aforementioned computational functions mainly rely on high-performance central processing units or digital signal processors based on software algorithms. In these solutions, the mathematical model of forward and inverse kinematics is executed sequentially on a general-purpose processor via program code, utilizing the processor's floating-point arithmetic capabilities to perform numerical calculations.

[0004] However, this software-based computation method using general-purpose processors has significant limitations. First, its computational efficiency is constrained by the processor's serial execution architecture. For complex inverse kinematics problems, the solution process is time-consuming, easily introducing computational latency and failing to meet the stringent real-time control requirements of high-speed, high-precision robots. Second, kinematic calculation tasks consume substantial processor resources, while the processor also needs to handle other system tasks such as trajectory planning, vision processing, and logical judgment, easily creating a system performance bottleneck and affecting the overall control system's responsiveness. Furthermore, in pursuit of shorter computation times, higher-performance processors are often required, directly leading to increased system hardware costs and power consumption.

[0005] Therefore, in order to solve the problems of computational latency, resource bottlenecks and cost in existing technologies, there is an urgent need for a brand-new technical solution that can achieve efficient parallel processing and table lookup mechanisms through dedicated hardware computing modules, so as to realize the real-time calculation of robot kinematics forward and inverse kinematics with higher efficiency and lower cost. Summary of the Invention

[0006] The technical problem to be solved by this invention is to address the shortcomings of existing technologies, specifically by providing a system and method for real-time forward and inverse kinematics calculation of robots, as detailed below:

[0007] 1) In a first aspect, the present invention provides a system for real-time forward and inverse kinematics calculation of a robot, the specific technical solution of which is as follows:

[0008] The system includes a data interface module, a forward calculus module, an inverse calculus lookup table module, and an error compensation module. The data interface module receives the target pose or preset angles for each joint from the robot's main control unit. The forward calculus module, upon receiving the preset angles for each joint, calculates the robot's end effector pose using parallel hardware calculations based on these preset angles. The inverse calculus lookup table module, upon receiving the target pose, retrieves the initial angles for each joint corresponding to the target pose using a lookup table. The error compensation module performs error compensation and singularity detection on the initial angles of each joint to obtain the target angle for each joint. The data interface module also sends the end effector pose or the target angles for each joint to the main control unit, enabling the main control unit to control the robot based on these poses.

[0009] The beneficial effects of the system for real-time forward and inverse kinematics calculation of robots provided by this invention are as follows:

[0010] By setting up a dedicated forward kinematics calculation module and employing hardware parallel computing to process the forward kinematics, the complex mathematical model is embedded in the hardware logic, achieving extremely high calculation speed. This fundamentally solves the computational latency problem caused by traditional software serial calculation methods, meeting the stringent real-time control requirements of high-speed, high-precision robots. For the more complex inverse kinematics problem, the system innovatively adopts a scheme combining an inverse kinematics lookup table module and an error compensation module. The inverse kinematics lookup table module directly obtains the initial angle of each joint corresponding to the target pose using a lookup table method, avoiding the huge computational load of solving complex nonlinear equations online in real time, thus improving the efficiency of inverse kinematics calculation. The error compensation module performs error compensation and singularity detection on the initial angle of each joint, performing fine correction through a pre-calibrated error mapping table and effectively avoiding kinematic singular configurations, thereby ensuring the accuracy of the inverse kinematics results and the robustness of the system. This solution removes computationally intensive tasks from the general-purpose main control unit and assigns them to dedicated hardware modules, greatly reducing the computational burden on the main control unit, successfully breaking through resource bottlenecks, and reducing dependence on the performance of the main control unit. This allows the use of a lower-cost, lower-power main control unit to build the entire robot control system, improving the system's cost-effectiveness while ensuring excellent real-time performance.

[0011] Based on the above scheme, the system for real-time forward and inverse kinematics calculation of robots according to the present invention can be further improved as follows.

[0012] Furthermore, the inverse kinematics lookup table module is specifically used to: query a pre-stored correspondence table between poses and joint angles based on the received target pose; if a pose identical to the target pose is found in the correspondence table, the angle of each joint corresponding to that identical pose is directly used as the initial angle; if no pose identical to the target pose is found in the correspondence table, at least two reference poses closest to the target pose and the reference angle of each joint corresponding to each reference pose are determined, and interpolation is performed on the reference angles of each joint corresponding to the at least two reference poses to calculate the initial angle of each joint.

[0013] The advantages of adopting the above-mentioned further scheme are as follows: When the target pose can find a complete match in the pre-stored correspondence table, the inverse kinematics lookup module can directly output the angle of each joint as the initial angle. This completely avoids the complex real-time numerical solution process, thereby achieving extremely high lookup speed and deterministic computational latency, greatly improving the real-time performance of inverse kinematics calculation. When a complete match cannot be found, by finding at least two reference poses closest to the target pose and performing interpolation, the true solution can be effectively approximated. This overcomes, to some extent, the accuracy loss problem that may be caused by the limited storage capacity of the correspondence table. This strategy of combining precise matching and intelligent interpolation ensures computational efficiency while also taking into account the accuracy of the results. It eliminates the need for the system to store all pose mapping relationships in the entire workspace, thus achieving an optimized balance between computational speed, accuracy, and storage resource consumption.

[0014] Furthermore, the data density of the correspondence table is configured according to the accuracy requirements of different areas in the robot's workspace.

[0015] The beneficial effects of adopting the above-mentioned further scheme are as follows: By configuring the data density of the correspondence table according to the accuracy requirements of different areas in the robot's workspace, intelligent allocation of storage resources is achieved. For high-precision areas where the robot needs to perform precise operations, the system can configure a higher data density and store denser pose and joint angle mapping points. This allows for the acquisition of extremely high-precision inverse kinematics results within these areas using a lookup table method, reducing reliance on interpolation operations and improving computational speed and local accuracy. Conversely, in auxiliary work areas where absolute accuracy requirements are relatively low, the system can configure a lower data density and store sparser mapping points, effectively reducing the occupation of valuable storage resources. This adaptive data density configuration mechanism ensures that, under limited storage capacity constraints, priority can be given to ensuring the motion control accuracy of critical work areas while also considering the coverage of the entire workspace, thus achieving an optimal balance between storage efficiency and overall inverse kinematics accuracy at the global level.

[0016] Furthermore, the error compensation module is used to: find the error compensation value associated with the initial angle of each joint based on a pre-calibrated error mapping table, and add the error compensation value of each joint to the initial angle to obtain the compensated angle of each joint; determine whether the compensated angle of each joint corresponds to the kinematic singular configuration of the robot, and obtain the judgment result; when the judgment result is negative, the compensated angle of each joint is taken as the target angle of each joint; when the judgment result is positive, the compensated angle of each joint is processed again through a preset singularity processing strategy, and the compensated angle of each joint is taken as the target angle of each joint.

[0017] The beneficial effects of adopting the above-mentioned further scheme are as follows: the error compensation module performs error compensation on the initial angle of each joint based on a pre-calibrated error mapping table, which can specifically correct systematic errors or model deviations introduced by the table lookup and interpolation process, thereby improving the absolute accuracy of the target angle of each joint. The subsequent singularity detection step is a core safety measure. By judging whether the compensated angle corresponds to the robot's kinematic singular configuration, it can promptly identify singular configuration risks that may lead to robot loss of control or damage. By performing branch processing based on the judgment result, a high-precision result is directly output in the safe state where the judgment result is negative, and a preset singularity handling strategy is activated for correction in the risk state where the judgment result is positive. This mechanism ensures that the system can output a stable and executable control command under any operating condition. This design enables the efficient but potentially accuracy-limited scheme of table lookup to be stably applied to high-requirement real-time control scenarios, effectively ensuring the robustness and safety of the system.

[0018] 2) In a second aspect, the present invention also provides a method for calculating the forward and inverse kinematics of a robot in real time, the specific technical solution of which is as follows:

[0019] The robot receives the target pose or preset angle of each joint from the robot's main control unit. When the preset angle of each joint is received, the robot obtains the end effector pose based on the preset angle of each joint and through parallel hardware calculation. When the target pose is received, the robot obtains the initial angle of each joint corresponding to the target pose by looking up a table. Error compensation and singularity detection are performed on the initial angle of each joint to obtain the target angle of each joint. The end effector pose or the target angle of each joint is sent to the main control unit so that the main control unit can control the robot according to the end effector pose or the target angle of each joint.

[0020] Based on the above scheme, the method for calculating forward and inverse kinematics of a robot in real time according to the present invention can be further improved as follows.

[0021] Furthermore, the initial angle of each joint corresponding to the target pose is obtained by a lookup table method, including: querying a pre-stored correspondence table between pose and joint angle based on the received target pose; if a pose identical to the target pose is found in the correspondence table, the angle of each joint corresponding to the identical pose is directly used as the initial angle; if no pose identical to the target pose is found in the correspondence table, at least two reference poses closest to the target pose and the reference angle of each joint corresponding to each reference pose are determined, and interpolation is performed on the reference angles of each joint corresponding to the at least two reference poses to calculate the initial angle of each joint.

[0022] Furthermore, the data density of the correspondence table is configured according to the accuracy requirements of different areas in the robot's workspace.

[0023] Furthermore, error compensation and singularity detection are performed on the initial angle of each joint to obtain the target angle of each joint. This includes: finding the error compensation value associated with the initial angle of each joint based on a pre-calibrated error mapping table, and adding the error compensation value of each joint to the initial angle to obtain the compensated angle of each joint; determining whether the compensated angle of each joint corresponds to the kinematic singular configuration of the robot, and obtaining the determination result; when the determination result is negative, the compensated angle of each joint is taken as the target angle of each joint; when the determination result is positive, the compensated angle of each joint is processed again through a preset singularity processing strategy, and the compensated angle of each joint is taken as the target angle of each joint.

[0024] 3) In a third aspect, the present invention also provides an electronic device, the electronic device including a processor coupled to a memory, the memory storing at least one computer program, the at least one computer program being loaded and executed by the processor, so that the electronic device implements any of the above-mentioned methods for real-time forward and inverse kinematics calculation of a robot.

[0025] 4) In a fourth aspect, the present invention also provides a computer-readable storage medium storing a computer program, wherein the computer program, when executed by a processor, implements any of the above-described methods for calculating forward and inverse kinematics of a robot in real time.

[0026] It should be noted that the beneficial effects of the technical solutions of the second to fourth aspects of the present invention and their corresponding possible implementations can be found in the above description of the technical effects of the first aspect and its corresponding possible implementations, and will not be repeated here. Attached Figure Description

[0027] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments of the present invention will be briefly introduced below:

[0028] Figure 1 This is one of the structural schematic diagrams of a system for real-time forward and inverse kinematics calculation of a robot according to an embodiment of the present invention;

[0029] Figure 2 This is a second schematic diagram of the structure of a system for real-time forward and inverse kinematics calculation of a robot according to an embodiment of the present invention;

[0030] Figure 3 This is a schematic diagram of the timing control and instruction scheduling module;

[0031] Figure 4 This is a schematic diagram of the forward calculation module;

[0032] Figure 5 This is a schematic diagram of the inverse lookup table module;

[0033] Figure 6 This is a schematic diagram of the error compensation module.

[0034] Figure 7 This is a schematic diagram of a method for real-time forward and inverse kinematics calculation of a robot according to an embodiment of the present invention. Detailed Implementation

[0035] The principles and features of the present invention are described below. The examples given are only for explaining the present invention and are not intended to limit the scope of the present invention.

[0036] like Figure 1 As shown, an embodiment of the present invention provides a system for real-time forward and inverse kinematics calculation of a robot, comprising a data interface module, a forward kinematics calculation module, an inverse kinematics lookup table module, and an error compensation module;

[0037] The data interface module is used to receive the target pose or preset angle of each joint sent by the robot's main control unit;

[0038] The preset angle for each joint of the robot can be the current angle or set according to the actual situation.

[0039] The data interface module is used to receive the target pose or preset angles of each joint sent by the robot's main control unit. As the hub for communication between the system and the external main control unit, the data interface module's implementation first requires defining and implementing an efficient and reliable communication protocol. This protocol is typically based on a high-speed serial interface, such as SPI, I2C, or industrial Ethernet protocols like EtherCAT, to ensure low-latency data transmission.

[0040] After power-on initialization, the data interface module continuously listens to the communication bus, waiting for the main control unit to send data frames. Upon receiving a data frame, the data interface module first performs frame header verification and cyclic redundancy check to confirm the integrity and correctness of the data. If the verification fails, it requests a retransmission from the main control unit via the status register; if the verification passes, it parses the data frame. Specifically, based on the predefined instruction code or address segment in the data frame, it determines the type of the current input data. If the instruction code indicates a reverse engineering calculation request, it extracts the component data of the target pose from specific fields of the data frame. The component data of the target pose includes all parameters describing the precise state that the robot's end effector needs to achieve in three-dimensional space, specifically position coordinates and attitude parameters. The position coordinates consist of three components: X, Y, and Z, determining the translational position of a point in space relative to the reference coordinate system; while the attitude parameters describe the orientation of the robot arm's end effector, which can usually be represented by Euler angles (such as yaw, pitch, and roll) or a complete 3×3 rotation matrix. Position and orientation parameters together constitute the pose of the robot's end effector, thus uniquely determining the position and orientation of the robot's end effector relative to the base coordinate system.

[0041] If the instruction code indicates a positive solution calculation request, then an array of preset angles for each joint is extracted from the data frame. The parsed data is stored in the corresponding input data registers: the target pose data is stored in the target pose register, and the preset angle data for each joint is stored in the joint angle register.

[0042] After completing data parsing and storage, the data interface module sends a ready signal to the system's timing control logic or directly to the corresponding calculation module. For inverse kinematics calculation requests, this signal triggers the inverse kinematics lookup table module to read the target pose register and begin lookup calculation; for forward kinematics calculation requests, it triggers the forward kinematics calculation module to read the joint angle register and begin parallel calculation.

[0043] Throughout the calculation process, the data interface module maintains a handshake signal with the main control unit, indicating a "busy" state. Upon receiving an interrupt signal indicating the completion of calculation from the error compensation module or the forward calculation module, the data interface module reads the final data from the corresponding output result register. For inverse kinematics calculations, it reads the target angle of each joint; for forward kinematics calculations, it reads the end effector pose. Subsequently, the data interface module encapsulates these result data into output data frames conforming to the communication protocol format and sends them back to the main control unit through the physical interface, simultaneously deactivating the "busy" state and preparing to receive the next instruction.

[0044] The robot's master control unit refers to the core decision-making part of the robot's control system, typically a host computer, embedded microcontroller, or motion control card. It is responsible for performing advanced tasks such as trajectory planning, task scheduling, and external sensor information fusion, and for issuing specific kinematic calculation instructions to the system of this invention. The target pose refers to the expected state that the robot's end effector needs to achieve in Cartesian space; it is a six-degree-of-freedom description including position and orientation. The preset angles of each joint refer to the angle values ​​actually measured at the current moment for each joint axis of the robot, or the set of expected angle values ​​set by the master control unit.

[0045] The forward calculation module is used to: when a preset angle of each joint is received, calculate the robot's end effector pose based on the preset angle of each joint and through hardware parallel calculation;

[0046] The core of the forward kinematics calculation module is to directly embed the mathematical model of the robot's forward kinematics into a dedicated hardware circuit. After the data interface module writes a set of preset angles for each joint into the input register, the forward kinematics calculation module immediately starts the calculation process, specifically:

[0047] The received preset angles (usually in radians) for each joint are fed in parallel into multiple computational units (specifically, computational units employing the CORDIC algorithm). These computational units are configured in rotation mode to calculate the sine and cosine values ​​corresponding to each joint angle at high speed and in parallel. This hardware implementation replaces time-consuming floating-point trigonometric function library calls in software, improving computational efficiency. After obtaining the sine and cosine values ​​for all joints, the module performs matrix multiplication operations based on the link transformation matrix determined by the robot's Denavit-Hartenberg parameter table. In the hardware implementation, this operation is not performed as a single matrix multiplication, but is decomposed into multiple parallel computational pipelines targeting specific position and attitude components. Specifically, the calculation of the position component of the end effector in the X-axis direction is completed by a dedicated hardware pipeline, which consists of multiple multipliers and adders cascaded in the logical order of the kinematic formulas. Similarly, the nine elements of the rotation matrix used to determine the attitude are also calculated simultaneously by multiple similar parallel pipelines. By assigning dedicated computation paths to each position component and rotation matrix element, computational parallelism in the spatial dimension is achieved, thereby significantly improving the overall throughput.

[0048] The entire computation process exhibits high spatial parallelism and temporal pipeline characteristics. Multiple computational pipelines operate synchronously under a unified clock, with each pipeline register responsible for latching intermediate results and passing them to the next stage. This deep pipeline design allows the module to receive a new set of joint angle data and output a corresponding set of pose results every clock cycle, thus achieving extremely high computational throughput. The calculated end effector pose data, including position coordinates and attitude matrix elements, is ultimately latched into the module's output register. Simultaneously, the forward homing calculation module sends a computation completion interrupt signal to the data interface module, indicating that the forward homing calculation task has been completed.

[0049] In this invention, hardware parallel computing specifically refers to a computational method that utilizes multiple hardware computing units (such as multipliers, adders, and CORDIC units) within an application-specific integrated circuit (ASIC) or FPGA to simultaneously execute computational tasks. This computational method, through spatial resource replication and temporal pipelined processing, decomposes complex kinematic formulas into multiple concurrently executable sub-operations, thereby achieving a computational speed at the physical level far exceeding that of sequential software execution. The end effector pose describes the complete information of a robot's end effector's position and orientation in three-dimensional space. Position is determined by three translational coordinates relative to the origin of the base coordinate system, while pose describes the rotational relationship of the end effector's coordinate system relative to the base coordinate system, typically represented by rotation matrices or equivalent Euler angles.

[0050] The inverse kinematics lookup table module is used to: upon receiving the target pose, obtain the initial angle of each joint corresponding to the target pose using a lookup table method. Specifically:

[0051] 1) Based on the received target pose, query the pre-stored correspondence table between pose and joint angles. Specifically:

[0052] The inverse lookup table module contains an address generation unit specifically designed to map a continuous, multi-dimensional pose space to discrete physical memory addresses. Upon receiving the target pose, the address generation unit first normalizes and quantizes each component of the target pose. These components include position coordinates (such as X, Y, and Z coordinates) and parameters representing the pose (such as Euler angles). Based on a preset lookup granularity, the address generation unit converts each consecutive floating-point component value into an integer index number.

[0053] Subsequently, the address generation unit uses a specific encoding strategy to convert these multidimensional indices into a unique linear physical memory address. The encoding strategy can be to combine the index numbers of each component in a specific order, or to calculate using a lightweight hash function. The generated physical address directly points to the starting position in the on-chip memory where the corresponding joint angle data is stored.

[0054] Once the address is generated, the query control unit sends a read enable signal and the memory address to the memory. The memory is typically implemented using high-speed static random access memory (SRAM) blocks to ensure low-latency data access. After one or several clock cycles, the memory outputs the data stored at that address, which is a set of pre-calculated reference point data packets. This data packet contains the index information of the reference pose closest to the target pose and its corresponding complete set of joint angles.

[0055] Upon receiving the data packet output from the memory, the query and interpolation unit immediately compares the input target pose with the reference pose in the data packet. This comparison is achieved by calculating the differences between the target pose and each reference pose in each dimension. The purpose of the comparison is to confirm whether a perfectly matching pose exists: if the difference between a reference pose and each reference pose is zero in all dimensions, it is determined to be a perfect match. In this case, the joint angle corresponding to the reference pose is directly output as the initial angle, and the subsequent interpolation steps are skipped; if no perfect match is found, the interpolation calculation stage begins.

[0056] The process of obtaining the correspondence table between pose and joint angles is as follows:

[0057] Given the known parameters of the robot's kinematic model, systematic sampling is required within its entire workspace. This sampling process needs to be executed according to the final configured data density strategy, i.e., different sampling granularities are used for different accuracy requirements in different areas of the workspace, generating a large number of uniformly distributed or targeted sets of end effector pose points. For each pose point obtained through sampling, inverse kinematics is required for calculation. This calculation process is completed offline on a general-purpose computer, using either analytical inverse kinematics or numerical iterative methods to solve for all possible joint angle combinations corresponding to that pose point. It should be noted that for most robot configurations, a specific end effector pose usually corresponds to multiple sets of joint angle solutions. Subsequently, the optimal solution needs to be selected from these multiple solutions based on the robot's actual physical constraints. These constraints include, but are not limited to, limiting the motion angles of each joint, avoiding mechanical interference, and considering the continuity requirements of the motion trajectory. By applying these constraints, the most reasonable and most likely set of joint angles to be practically adopted is selected from multiple solutions, and a unique mapping relationship is established with the sampled pose. Finally, all calculated and optimized pose-joint angle data pairs are sorted and formatted according to pre-designed address mapping rules to generate a standardized data file that can be read by hardware. This data file is then programmed into the on-chip memory of the inverse kinematics lookup table module via the chip's programming interface, forming a real-time lookup table of the correspondence between pose and joint angles. This offline preparation ensures that lookup operations can be performed quickly during real-time calculations.

[0058] 2) If a pose identical to the target pose is found in the correspondence table, the angle of each joint corresponding to that identical pose is directly used as the initial angle.

[0059] The following is an example of controlling a 6-DOF robot. After the pre-stored mapping table is configured, the main control unit requests an inverse kinematics calculation. The target pose to be reached by the end effector is defined as: position coordinates (X=500.0mm, Y=0.0mm, Z=800.0mm), and the attitude is represented by Euler angles (A=0.0°, B=0.0°, C=90.0°). After the data interface module transmits this target pose data to the inverse kinematics lookup table module, the address generation unit within the module begins operation.

[0060] Assume the quantization precision of the position components (X, Y, Z) in this region is 1 mm, and the quantization precision of the attitude components (A, B, C) is 1 degree. The address generation unit first quantizes the target pose: the position coordinates X=500.0 mm, Y=0.0 mm, and Z=800.0 mm are converted into integer index numbers 500, 0, and 800, respectively; simultaneously, the attitude Euler angles A=0.0°, B=0.0°, and C=90.0° are converted into integer index numbers 0, 0, and 90, respectively.

[0061] Subsequently, the address generation unit concatenates six index numbers in a predetermined order to generate a unique physical address, such as Addr_500_0_800_0_0_90. The query control unit uses this address to access the on-chip memory and read the data packet stored at that address. This data packet contains reference pose data (quantized values: X_idx=500, Y_idx=0, Z_idx=800, A_idx=0, B_idx=0, C_idx=90) and the corresponding six joint angle values, such as [J1=30.5°, J2=-45.1°, J3=120.3°, J4=0.0°, J5=60.2°, J6=90.0°].

[0062] The query and interpolation unit performs an exact match determination, comparing the quantization index of the target pose (500, 0, 800, 0, 0, 90) with the read reference pose quantization index (500, 0, 800, 0, 0, 90) component by component. An exact match is determined when all index values ​​are identical. At this point, the query and interpolation unit directly uses the joint angle array [J1=30.5°, J2=-45.1°, J3=120.3°, J4=0.0°, J5=60.2°, J6=90.0°] read from the data packet as the initial angle output for each joint and transmits it to the subsequent error compensation module. This query and matching process can be completed within a few clock cycles, achieving highly efficient inverse lookup.

[0063] 3) If no pose matching the target pose is found in the correspondence table, determine at least two reference poses closest to the target pose and the reference angles of each joint corresponding to each reference pose. Then, perform interpolation on the reference angles of each joint corresponding to the at least two reference poses to calculate the initial angle of each joint. Specifically:

[0064] ① Using the physical address calculated by the address generation unit as the center, the module systematically searches for adjacent reference points in the memory address space. Since the pose space is multi-dimensional, containing six dimensions—X, Y, Z position coordinates and A, B, C pose angles—the query and interpolation unit needs to find adjacent indices in each dimension. Specifically, assuming the target pose quantization index is (X=505, Y=2, Z=795, A=2, B=2, C=88), and the nearest reference point index stored in memory is (X=500, Y=0, Z=800, A=0, B=0, C=90), the module will systematically generate and read the 2... n Data packets from several adjacent addresses (where n is the dimension number; for a six-dimensional space, there are usually 64 adjacent points, but in practical applications, the 8 or more closest key reference points are selected based on the storage structure and computational complexity) are used to obtain multiple adjacent reference poses and the reference angles of each joint.

[0065] ② The query and interpolation unit calculates the "distance" between the target pose and each acquired reference pose in multidimensional space. This distance calculation uses a weighted Euclidean distance method, which comprehensively considers the differences between the position and attitude components. During the calculation, the differences in attitude components are normalized and weighted according to their units (e.g., radians) and their importance in practical applications to ensure that position and attitude errors have reasonable contribution weights in the distance calculation. After completing the distance calculation for all reference poses, the unit selects the two reference poses with the shortest distance to the target pose (in some application scenarios, more reference points may be selected, depending on the complexity and accuracy requirements of the interpolation algorithm) as interpolation reference points. Assuming the two selected reference poses are Ref1 and Ref2, their corresponding reference angle arrays for each joint are J_ref1 and J_ref2, respectively.

[0066] ③ For each of the six joints, interpolation calculations are performed independently. Taking linear interpolation as an example, the module calculates the corresponding weight factors w1 and w2 based on the distance ratio of the target pose relative to Ref1 and Ref2. Then, the interpolation calculation is performed on the first joint: J1_initial = w1 × J1_ref1 + w2 × J1_ref2. The same interpolation operation is applied simultaneously to the other five joints through hardware parallel computing, with each joint having a dedicated interpolation calculation unit working independently. This parallel architecture allows the interpolation calculations of the six joints to be completed almost simultaneously, ultimately yielding a complete set of initial angles for each joint after interpolation calculations. The entire process is implemented through highly optimized hardware logic, completed within an extremely short clock cycle, effectively achieving continuous approximation between discrete stored data points, improving the accuracy and practicality of the lookup table method.

[0067] The data density of the correspondence table is configured according to the accuracy requirements of different areas in the robot's workspace, specifically:

[0068] ① During the system design phase, the robot's workspace needs to be analyzed and divided. Engineers divide the robot's reachable workspace into areas with different precision requirements based on the needs of the robot's actual application scenarios. For example, for a robot performing precision assembly tasks, its core working area for fine operations is defined as a high-precision requirement area; while non-operational areas or obstacle avoidance spaces used for rapid material transfer are classified as low-precision requirement areas.

[0069] ② When generating the correspondence table, the system employs differentiated sampling strategies for different regions to determine the distribution density of pose points. This differentiated strategy is directly reflected in the quantization accuracy or sampling interval settings of each pose component. For a six-DOF pose, its data density is a combination of the sampling intervals across the six dimensions (X, Y, Z position coordinates and A, B, C pose angles). In actual implementation, the density of point distribution in the pose space is controlled by configuring different quantization parameters for each region.

[0070] ③ At the hardware implementation level, this configurable data density characteristic requires the address generation unit of the inverse lookup table module to have programmable quantization logic. This unit internally stores a "region-quantization parameter" mapping table, which records the quantization parameters corresponding to each predefined workspace region. When processing a target pose, the address generation unit first determines its workspace region based on the pose's position coordinates (X, Y, Z), then automatically loads the quantization parameters corresponding to that region (including the interval values ​​of the position and angle components), and uses these parameters to quantize the target pose, thereby generating the query address. This mechanism means that the same physical coordinate value may be quantized into different index numbers in different regions, thus mapping to data blocks of different densities in memory.

[0071] Taking a six-axis robot used for assembly as an example, its workspace has a precision assembly area (high-precision area) at its center, while the surrounding area is a large material handling area (low-precision area). In the high-precision assembly area (e.g., X-coordinate within the range of [100mm, 200mm]), the system sets a high data density: the sampling interval for position components (X, Y, Z) is set to 1 mm, and the sampling interval for attitude components (A, B, C) is set to 1 degree. This means that in this area, for every 1 mm movement or 1 degree rotation of the end effector, an independent pose record point is established in the corresponding relationship table, ensuring that the reference points are densely distributed when looking up the table, the interpolation error is minimal, and the accuracy requirements of precision assembly are met. In the low-precision material handling area (e.g., X-coordinate within the range of [500mm, 1000mm]), to save storage space, the system sets a lower data density: the sampling interval for position components is set to 10 mm, and the sampling interval for attitude components is set to 5 degrees. In this configuration, a record point is established in the table only when the end of the machine moves 10 mm or rotates 5 degrees. Although the potential interpolation error of a single query is relatively increased, it is within the allowable range because the handling task has low requirements for absolute positioning accuracy. At the same time, it reduces the number of pose-joint angle pairs that need to be stored in this area.

[0072] By employing this data density configuration method based on regional accuracy requirements, an optimal balance between storage efficiency and application performance is achieved within limited memory capacity. Ultimately, this correspondence table with non-uniform data density is programmed into the chip's on-chip memory, providing optimized data support for real-time inverse lookup table derivation.

[0073] The error compensation module is used to: compensate for errors in the initial angle of each joint and detect singularities to obtain the target angle of each joint. Specifically:

[0074] 1) Based on a pre-calibrated error mapping table, find the error compensation value associated with the initial angle of each joint, and add the error compensation value of each joint to the initial angle to obtain the compensated angle of each joint. Specifically:

[0075] After receiving the initial angle array for each joint from the inverse lookup table module, the error compensation module's internal data processing unit first quantizes these consecutive joint angle values. This quantization process strictly adheres to the quantization precision used when establishing the error mapping table, converting each joint angle value into an integer index value. For example, if the error mapping table sets the quantization precision for the first joint to 0.1 degrees, then the initial angle J1 = 30.52 degrees for each joint will be quantized to index 305. The error compensation module uses this composite address, composed of the quantized indices of the six joints, to query the pre-calibrated error mapping table. This error mapping table is typically stored in a dedicated on-chip read-only memory or a configurable RAM block. The goal of the query is to obtain the angle error compensation value that needs to be applied to each joint axis under the current joint angle configuration. There are two possible scenarios in the query process:

[0076] ① The first case is that the query address has a completely matching entry in the error mapping table. In this case, the module directly reads the compensation values ​​of the six joints stored at that address, for example, ΔJ1=+0.03 degrees, ΔJ2=-0.01 degrees, ..., ΔJ6=+0.05 degrees.

[0077] ② The second scenario is when the query address does not have a completely matching entry in the error mapping table. In this case, the module employs a strategy similar to inverse lookup, searching for the closest reference points. For example, for index 305 of J1, the error mapping table might only store data at indices 300 and 310. The module retrieves the error compensation values ​​of these adjacent reference points and then performs linear interpolation based on the initial angle of each joint relative to the specific position of these reference points to obtain the accurate compensation value for the current point. For all six joints, this interpolation process is executed in parallel to ensure processing efficiency.

[0078] After obtaining the error compensation values ​​for all six joints, the compensation execution unit performs a compensation operation, adding the error compensation value of each joint to its corresponding initial angle: J_compensated = J_initial + ΔJ. This addition operation is performed by hardware-implemented joint-by-joint, parallel addition.

[0079] For example, assuming the initial angles of each joint are [J1=30.52°, J2=-45.08°, J3=120.35°], and the quantization precision of the error mapping table is 0.1 degrees per joint, the resulting indexes are [305, -451, 1204]. The error compensation module uses this index to query the error mapping table and finds no perfect match. The module then performs linear interpolation based on the relative position ratio of the initial angle values ​​between adjacent reference points. For example, for J1=30.52°, it lies between indices 300 (30.0°) and 310 (31.0°), with a position ratio of 0.2. Assuming the compensation value at index 300 is 0.02 degrees and the compensation value at index 310 is 0.04 degrees, then ΔJ1 is calculated as 0.02 + (0.04 - 0.02) × 0.2 = 0.024 degrees. The compensated angle J1_compensated = 30.52 + 0.024 = 30.544 degrees. The compensated angles of the remaining joints are calculated in parallel using the same principle, ultimately yielding an array of compensated angles for all joints.

[0080] The error mapping table records the deviations between the robot's actual kinematic parameters and the ideal model under different joint configurations. The acquisition process is as follows:

[0081] ① A high-precision measurement environment needs to be established. Specifically, the robot should be securely mounted on a base, and a high-precision measurement target or a cooperative target with a laser tracker should be installed on its end effector. Simultaneously, an external measurement device with a measurement accuracy far exceeding the robot's repeatability, such as a laser tracker or a high-precision vision measurement system, should be used. This measurement device is used to establish an absolutely accurate world coordinate system and can acquire the true three-dimensional coordinates of the target point on the end effector in real time and with high precision.

[0082] ② Control the robot to move according to a pre-planned program, so that its end effector traverses multiple representative joint angle combinations within the workspace. For each preset joint angle combination, such as [J1=θ1, J2=θ2, ..., J6=θ6], two key operations are executed simultaneously: First, the actual angle measurement values ​​of each joint are accurately recorded by the high-resolution encoders of each joint; Second, a high-precision external measuring device is immediately triggered to acquire the true position coordinates (X_actual, Y_actual, Z_actual) of the end effector target center in the absolute world coordinate system.

[0083] ③ For each acquired data point, the theoretical joint angles [θ1, θ2, ..., θ6] recorded by the encoder are input into the robot's ideal forward kinematics model to calculate the theoretical pose (X_theoretical, Y_theoretical, Z_theoretical) corresponding to the end effector. Next, the calculated theoretical position coordinates are compared with the actual measured position coordinates to calculate the position error vector in three-dimensional space. Since the root cause of this error lies in the deviation between the robot's actual kinematic parameters (such as link length, link offset, joint zero position, link torsion angle, etc.) and the ideal model, it is necessary to use an error model (e.g., the differential transformation method based on the Jacobian matrix) to inversely decompose, map, or parameterize the end effector's position error onto each joint axis, and finally solve for the compensation value [Δθ1, Δθ2, ..., Δθ6] required to compensate for this pose error at each joint angle.

[0084] ④ The collected large amount of [joint angle combinations, joint angle compensation values] data is systematically organized and analyzed. To facilitate hardware table lookup, the continuous joint space needs to be discretized and quantized, i.e., the index step size of each joint is determined (e.g., 0.1 degrees). Then, all data points are sorted and categorized according to the quantized joint angle index. For multiple data points with the same index, averaging or curve fitting optimization can be performed, ultimately generating a discretized error lookup table covering the robot's main workspace. This complete error mapping table is finally burned into the non-volatile memory of a dedicated chip, providing accurate error lookup basis for the error compensation module during real-time operation.

[0085] 2) Determine whether the compensated angle of each joint corresponds to the kinematic singular configuration of the robot, and obtain the determination result;

[0086] Among them, the singular kinematic configuration refers to the state in which the Jacobian matrix of a robot is rank deficient when it is in certain special joint angle combinations in its workspace, that is, the determinant value is zero or close to zero. In this state, the robot's end effector will lose its instantaneous motion capability in some degrees of freedom in Cartesian space. This manifests as the joint velocity approaching infinity in order to maintain the movement in a certain direction of the end effector, or conversely, the end effector cannot move in certain directions, which leads to control instability, a sharp drop in accuracy, or even mechanical damage.

[0087] The process of determining whether the compensated angle of each joint corresponds to a singular kinematic configuration of the robot and obtaining the result is based on a fundamental mathematical property of robot kinematics: near singular points, the condition number of the Jacobian matrix, which describes the linear transformation between joint velocities and end effector velocities, becomes extremely large, or its determinant approaches zero. Therefore, the core of hardware detection is to quickly calculate a criterion related to the properties of the Jacobian matrix. The specific implementation process is as follows:

[0088] ① After obtaining the compensated joint angle array, the error compensation module immediately sends it to a dedicated singularity detection and calculation unit. This unit consists of hardware logic circuits, internally containing the analytical expression of the Jacobian matrix for this specific robot model. The unit operates using a parallel and pipelined architecture. It first calculates the values ​​of all elements in the Jacobian matrix in parallel based on the input six joint angle values. This calculation process involves a large number of trigonometric function operations (completed by the calculation unit) and multiplication operations.

[0089] ② After generating the complete Jacobian matrix, the detection unit does not directly calculate the determinant of this 6x6 matrix (for a 6-DOF robot, the Jacobian matrix is ​​6x6, and directly calculating the determinant is very complex), but instead uses a more efficient criterion. For example, for common wrist singularities (related to the 4th, 5th, and 6th axes), the detection unit may only calculate the determinant of the 3×3 submatrix consisting of the last three rows and three columns, which is related to the wrist pose. Alternatively, it may calculate a scalar that better reflects the singularity, such as the square of the smallest singular value of the Jacobian matrix.

[0090] ③ The calculated criterion value is immediately compared with one or more thresholds pre-stored in registers. These thresholds are determined through offline theoretical analysis and experimental testing to define safe regions, warning regions, and singular regions. The comparator outputs a result: if the criterion value is higher than the safe threshold, the result is negative, meaning it is not in a singular configuration; if the criterion value is lower than the singular threshold, the result is positive, meaning it is in or very close to a singular configuration; there may also be an intermediate region that triggers a warning state.

[0091] Consider a common wrist singularity in a six-DOF robot as an example. When the robot's fifth joint (J5) angle approaches 0 degrees, the rotation axes of the fourth joint (J4) and the sixth joint (J6) tend to be collinear, causing the wrist to lose one rotational degree of freedom. Assume the compensated joint angles are [J1=10°, J2=20°, J3=30°, J4=40°, J5=0.5°, J6=50°]. The singularity detection unit quickly calculates the pose Jacobian submatrix associated with the J4, J5, and J6 axes. This unit calculates the determinant of this 3×3 submatrix. Since the determinant is proportional to sin(J5), when J5 approaches 0 degrees, sin(J5) also approaches 0, resulting in a very small determinant value. Assume the calculated value here is 0.008. The preset wrist singularity threshold is 0.01. Since 0.008 < 0.01, the comparator determines that the current condition triggers a singularity. It outputs a "yes" result and passes this Boolean signal along with the current criterion value to the subsequent logic processing unit to activate the preset singularity handling strategy.

[0092] 3) If the judgment result is negative, the compensated angle of each joint is taken as the target angle of each joint; if the judgment result is positive, the compensated angle of each joint is processed again using a preset singularity handling strategy, and the compensated angle of each joint is taken as the target angle of each joint. Specifically:

[0093] When the judgment result is negative, it indicates that the robot configuration corresponding to the current compensated joint angle is far from the kinematic singularity and is in a safe and controllable state. At this time, the system's internal state machine will directly execute the standard processing path: generate control signals to select the compensated joint angle data stream and use it as the final target angle output for each joint. At the same time, the state machine will ensure that the correction calculation unit prepared for the singularity handling strategy is bypassed or reset to avoid introducing unnecessary power consumption and computational delay. This is the most commonly used and most efficient data path in the system.

[0094] When the judgment result is yes, the state machine immediately triggers the preset singularity handling procedure. The system adopts an appropriate intervention strategy based on the detected singularity type. One typical hardware-friendly strategy is the joint space micro-offset method. The execution unit of this strategy receives the compensated joint angles and singularity type information (e.g., wrist singularity or arm singularity), and then applies a predefined micro-correction to one or two specific joint angles according to the singularity type. For example, for common wrist singularities, the processing unit adds an offline-optimized offset (e.g., 0.5°) to the fifth joint angle, which is close to zero degrees. The selection of this offset is precisely calculated to ensure that the relevant criteria value of the Jacobian matrix quickly moves out of the danger zone at the cost of minimal end-effector pose error.

[0095] After the correction calculation is completed, the system performs a crucial secondary verification step: feeding back the corrected joint angles to the singularity detection unit for a new singularity assessment. If the secondary verification result becomes negative, it indicates that the correction is effective, and the state machine outputs the corrected joint angles as the final target angles for each joint. If the result is still positive, the processing strategy may iterate once (applying a larger offset) or trigger a high-level warning (such as sending an error code to the main control unit). The entire decision-making and correction process is completed by hardware logic within a very short clock cycle.

[0096] For example, when the compensated joint angles are [J1=10°, J2=20°, J3=30°, J4=40°, J5=0.5°, J6=50°] and the judgment result is "yes" (wrist singularity), the processing unit identifies it as a wrist singularity and mainly adjusts the J5 joint. The unit reads the preset offset + 0.7° from the register, calculates the corrected J5 angle as 0.5° + 0.7° = 1.2°, and obtains a new joint angle group [J1=10°, J2=20°, J3=30°, J4=40°, J5=1.2°, J6=50°]. This new angle group is sent back to the singularity detection unit to recalculate the determinant value of the Jacobian submatrix. Since sin(1.2°) is much larger than sin(0.5°), the determinant value increases from 0.008 to 0.015, exceeding the threshold of 0.01, and the secondary verification judgment result is "no". The state machine therefore outputs the corrected set of angles as the final target angle for each joint.

[0097] The data interface module is also used to send the end effector pose or the target angle of each joint to the main control unit, so that the main control unit can control the robot according to the end effector pose or the target angle of each joint.

[0098] Once the forward solving module or error compensation module completes its calculation task, it sends a clear calculation completion interrupt signal to the data interface module. Upon receiving this interrupt signal, the state machine within the data interface module immediately switches from an idle or waiting state to a data transmission preparation state. First, the module needs to accurately determine the source of the data to perform the correct multiplexing. The state machine checks the source identifier of the interrupt signal: if the interrupt originates from the forward solving module, the multiplexer within the module switches to the output result register of the forward solving module, preparing to read the end effector pose data; if the interrupt originates from the error compensation module, the multiplexer switches to the output result register of the error compensation module, preparing to read the target angle data for each joint.

[0099] After the data source is identified and the corresponding result data is read, the module's communication protocol encapsulation unit begins operation. This unit encapsulates the read raw data into a complete data frame according to a pre-defined communication protocol format with the main control unit. This data frame has strict format specifications, typically including a frame header, instruction / acknowledgment code, data payload length, actual data payload (i.e., end effector pose data or target angle arrays for each joint), and a frame tail and cyclic redundancy check (CRC) code. The instruction or acknowledgment code explicitly identifies to the main control unit whether the returned data is a forward or inverse calculation result, enabling the main control unit to perform correct subsequent processing.

[0100] After the data frame is encapsulated, it is not sent immediately. The module first checks the status of the hardware handshake signal line with the master control unit to confirm whether the master control unit is in a "ready to receive" state. If the master control unit is not detected to be ready, the data interface module will enter a waiting state and retry the handshake after a preset timeout. Once the master control unit is confirmed to be ready, the module's physical layer transmission logic is immediately started, converting the parallel data into a serial bit stream, and sending the data frame bit by bit through physical interfaces such as SPI, I2C, or EtherCAT at a set baud rate or a higher transmission rate.

[0101] Throughout the data transmission process, the data interface module informs the system that it is in a communication state via a specific status bit (e.g., a busy signal). In this state, the module temporarily refrains from responding to new computation requests to ensure the integrity and sequential transmission of the current data stream. Once the entire data frame has been successfully transmitted, the module clears the busy flag, resets its internal state machine, and re-enters the waiting-to-receive state, preparing to receive the next instruction from the main control unit. This process is implemented through dedicated hardware logic, ensuring extremely high response speed and transmission reliability, enabling the main control unit to promptly obtain computation results and accurately drive the robot to complete the corresponding motion control tasks.

[0102] In another embodiment, a detailed description is given using a 6-DOF serial robotic arm as an example, illustrating the specific hardware architecture and workflow of the system used for real-time forward and inverse kinematics calculations of the robot. Specifically:

[0103] The system's hardware architecture mainly includes a data interface module, a forward calculus module, an inverse calculus lookup table module, and an error compensation module. The data interface module receives instructions and data from the robot's main control unit, specifically the target pose for inverse calculus calculation or the current joint angles for forward calculus calculation. The forward calculus module consists of multiple dedicated hardware computing units, including hardware multipliers, adders, triggers, and a dedicated CORDIC unit for high-speed trigonometric function calculation. This module takes the received joint angles as input and, through a carefully designed parallel pipeline processing architecture, achieves hardware parallel computation, ultimately outputting the complete pose information of the robot's end effector, namely its position coordinates and attitude parameters.

[0104] The inverse kinematics lookup table module is the core component for handling inverse kinematics problems, and it contains several key sub-units. The storage unit is implemented using high-capacity on-chip static memory to store a pre-calculated and optimized table of correspondences between poses and joint angles. The lookup and interpolation unit receives the target pose and quickly searches the memory for the closest reference point based on that pose. To improve calculation accuracy, when a perfectly matching pose is not found, this unit performs linear or nonlinear interpolation to calculate the initial angle of each joint. Furthermore, this module supports flexible configuration of the correspondence table data density. During chip programming or configuration, different sampling granularities can be set according to the accuracy requirements of different areas in the robot's workspace. For example, high-density sampling points can be set in high-precision core working areas, while lower-density sampling points can be set in secondary areas with relatively lower accuracy requirements, thereby optimizing the use of storage resources.

[0105] The error compensation module integrates both error compensation and singularity detection and processing functions. Based on a pre-calibrated error mapping table, the error compensation unit finely adjusts the initial angles of each joint obtained by the inverse lookup table module to effectively reduce the overall deviation caused by factors such as lookup granularity, interpolation approximation, and manufacturing and assembly errors of the robot body. The singularity detection function uses dedicated hardware logic to determine whether the compensated joint angles correspond to the robot's kinematic singular configuration. When a singularity is detected, a preset singularity processing strategy is automatically triggered to adjust the joint angles online, ensuring the stability and safety of the system under various operating conditions.

[0106] The system's workflow is clearly divided into two independent paths: inverse kinematics (IK) calculation and forward kinematics (TK). During IK calculation, the main control unit sends the desired target pose to the system via the data interface module. The IK lookup module queries a pre-stored table based on the target pose and obtains the initial angle of each joint through efficient lookup and interpolation. Subsequently, the error compensation module performs system error compensation and rigorous singularity detection and processing on this initial angle to obtain the final optimized target angle for each joint. Finally, the data interface module sends the calculated target angles back to the main control unit for real-time driving of the robot's joint servo motors. During TK calculation, the main control unit sends the robot's current joint angle values ​​to the system. The TK module performs high-speed calculations using its highly parallel hardware pipeline structure, outputting the current pose of the robot's end effector almost instantaneously. This pose data is also returned to the main control unit via the data interface module for real-time robot status monitoring or closed-loop feedback control. The entire system achieves extremely fast and high-precision calculation of forward and inverse kinematics of the robot through the collaborative work of various dedicated hardware modules and the core mechanisms of hardware parallel computing and intelligent lookup table method. It effectively solves the problems of computational latency, resource bottleneck and high cost inherent in traditional software solutions based on general-purpose processors.

[0107] Figure 2 This paper demonstrates the internal architecture of a system (typically implemented as a dedicated forward and inverse kinematics algorithm chip) for real-time forward and inverse kinematics calculations of a robot, and its connection to an external robot control unit (MCU). As shown in the figure, the MCU, as the core controller of the entire robot control system, communicates with the dedicated chip of this invention through a high-speed switching data interface supporting SPI or I2C dual-mode switching. Internally, the timing control and instruction scheduling module acts as the core coordinator of the entire system. It receives instructions and input data from the data interface module via the LocalBus internal bus and performs data scheduling based on the instruction type (forward or inverse kinematics request): scheduling joint angle data to the forward kinematics calculation module, or scheduling target pose data to the inverse kinematics lookup table module. The forward kinematics calculation module outputs the end effector pose through a dedicated hardware parallel computing unit, while the inverse kinematics lookup table module outputs the initial angle of each joint through efficient lookup and interpolation operations. The calculation results of both are sent to the error compensation and robustness module (i.e., the error compensation module) for subsequent error compensation and singularity detection and processing. The final processing result is then controlled by the timing control and instruction scheduling module and sent back to the main control MCU through the LocalBus internal bus and data interface module, thus completing a complete forward or inverse solution calculation task.

[0108] Figure 3The detailed internal structure of the timing control and instruction scheduling module is shown in detail. As the system's control hub, this module contains key components such as input registers, instruction decoders, state machine controllers, timing generators, activation modules for multiple computation modules, and output registers. Its workflow begins with instructions and data received by the data interface module being temporarily stored in the input registers. Subsequently, the instruction decoder parses the instructions to distinguish between forward and inverse decoding requests. Based on the decoding results, the state machine controller, in conjunction with a unified master clock generated by the timing generator and specific register read / write enable signals, guides the scheduling process of the entire system. This controller sends activation signals to the corresponding computation modules (forward decoding modules or inverse decoding lookup modules) and continuously monitors their execution status. During computation, the computation modules provide real-time feedback to this module, including status signals such as "computation busy" or "completion interrupted," as well as crucial intermediate computation data like the determinant value of the Jacobian matrix, to characterize their current working state and computation progress. The state machine controller integrates these external feedback signals, internal intermediate data, and its own status signals to precisely control the timing of each operation. When the calculation is complete, the result is latched into the output register and returned to the main control unit via the data interface under the control of this module.

[0109] like Figure 4 As shown, the detailed hardware implementation flow of the forward kinematics calculation module is as follows: It begins with the joint angle data being stored in the input register. Subsequently, the trigonometric function calculation unit calculates the sine (sin) and cosine (cos) values ​​of each joint angle in parallel based on the efficient CORDIC algorithm. The matrix generation unit uses these trigonometric function calculation results and the robot's Denavit-Hartenberg (DH) parameters to quickly generate the homogeneous transformation matrix for each link. These transformation matrices are then fed into the matrix multiplication pipeline, which consists of multiple stages of hardware multipliers and adders. Through deep pipeline technology, continuous matrix multiplication is decomposed into multiple overlapping execution stages for parallel computation, gradually obtaining the final pose transformation matrix of the end effector relative to the robot's base coordinate system. The calculated pose data (including position and orientation) is latched into the output register, awaiting reading by the timing control and instruction scheduling module and returned to the main control unit.

[0110] Figure 5The internal workflow of the inverse kinematics lookup table module is described in detail. It begins with the target pose data being stored in the input register. Subsequently, the adaptive lookup table control unit, based on the workspace region where the target pose is located, calls the corresponding quantization parameters to quantize each component of the pose (X, Y, Z, A, B, C) and generates a lookup address. The storage unit, as the core component, internally stores a complete table of the correspondence between pose and joint angles. Based on the generated lookup address, it outputs the corresponding reference pose data and related joint angle data. If no exact match is found in the table, the interpolation calculation unit uses the neighboring reference point data read from memory to perform linear or nonlinear interpolation calculations to calculate the initial angle of each joint corresponding to the current target pose. The final calculation results are temporarily stored in the output register for subsequent error compensation module reading and processing.

[0111] Figure 6 This demonstrates the complete data processing flow of the error compensation module (also known as the error compensation and robustness module). It begins by storing the initial angle data of each joint, output from the inverse lookup table module, into the input register. Subsequently, the error compensation unit, based on a pre-calibrated error mapping table, queries and interpolates the initial angle of each joint to obtain the error compensation value for each joint. These compensation values ​​are then added to the corresponding initial angle to obtain the compensated joint angle. Next, the singularity avoidance unit performs kinematic singularity configuration judgment on the compensated joint angles. If a singularity is detected, a preset singularity handling strategy is immediately triggered to adjust the joint angles online, resulting in a final, safe, and feasible target angle for each joint. All processed results are latched into the output register, awaiting return to the main control unit.

[0112] This invention provides a system for real-time forward and inverse kinematics calculation of robots. Its core innovation lies first in its all-hardware implementation, directly embedding the forward and inverse kinematics calculation algorithms into the hardware logic of an application-specific integrated circuit (ASIC) or a field-programmable gate array (FPGA). Second, for the computationally complex inverse kinematics problem, it innovatively employs a lookup table method as the core solution, pre-storing a large number of correspondence tables between the end effector poses and the robot's joint angles, optimized through offline computation, within the chip's storage module. Third, the system supports dynamic and adaptive lookup table strategies, allowing users to flexibly adjust the data density and sampling granularity of the correspondence tables in different working areas based on the trade-offs between computational accuracy and storage space resources required by the actual application scenario during chip programming or system configuration. Fourth, the system integrates a fully functional error compensation module, which simultaneously possesses calibration-based error compensation and real-time singularity detection and processing capabilities. It corrects the lookup and interpolation results by querying the error mapping table and improves the system's robustness under complex working conditions through intelligent singularity handling strategies. Fifth, forward calculus is accelerated in parallel through highly pipelined dedicated hardware computing units, enabling continuous computation in a pipelined manner within a defined number of clock cycles, significantly improving the system's computational throughput. Finally, the system provides a standardized high-speed data interface module, allowing for seamless connection and efficient communication with various types of microcontrollers, digital signal processors, or personal computers. The technical effects are as follows:

[0113] First, the system achieves extremely high real-time performance. Hardware parallel computing and lookup table methods fundamentally eliminate the computational latency caused by sequential software execution, enabling microsecond-level forward and inverse kinematics calculations, fully meeting the stringent real-time control requirements of modern high-speed, high-precision robots. Second, the system possesses both high precision and high flexibility. Through an adaptive lookup table strategy and a refined error compensation mechanism, it effectively utilizes limited storage resources while maintaining high precision, successfully overcoming the contradiction between wasted storage resources and insufficient computational accuracy inherent in traditional single fixed lookup table methods. Third, the system exhibits high robustness. The real-time singularity detection and intelligent processing functions integrated into the error compensation module greatly improve the system's stability and safety under complex working trajectories and critical configurations. Fourth, the system effectively frees up the computing resources of the main control unit, transferring the computationally intensive forward and inverse kinematics tasks entirely to dedicated hardware, reducing the computational burden on the main control unit and allowing it to focus more on higher-level decision-making and control tasks such as trajectory planning, environmental perception, and visual processing. Fifth, the system has a cost-performance advantage. Compared with relying on expensive high-performance general-purpose processor solutions, using this dedicated system can reduce the overall hardware cost and system power consumption, while providing superior computing performance far exceeding that of software solutions.

[0114] like Figure 7As shown in the figure, a method for calculating the forward and inverse kinematics of a robot in real time according to an embodiment of the present invention includes the following steps:

[0115] S1. Receive the target pose or preset angle of each joint sent by the robot's main control unit;

[0116] S2. When the preset angle of each joint is received, the robot's end effector pose is obtained by parallel hardware calculation based on the preset angle of each joint. When the target pose is received, the initial angle of each joint corresponding to the target pose is obtained by table lookup.

[0117] S3. Perform error compensation and singularity detection on the initial angle of each joint to obtain the target angle of each joint;

[0118] S4. Send the end effector pose or the target angle of each joint to the main control unit so that the main control unit can control the robot according to the end effector pose or the target angle of each joint.

[0119] Optionally, in the above technical solution, the initial angle of each joint corresponding to the target pose is obtained by a lookup table method, including: querying a pre-stored correspondence table between pose and joint angle based on the received target pose; if a pose identical to the target pose is found in the correspondence table, the angle of each joint corresponding to the identical pose is directly used as the initial angle; if no pose identical to the target pose is found in the correspondence table, at least two reference poses closest to the target pose and the reference angle of each joint corresponding to each reference pose are determined, and interpolation is performed on the reference angles of each joint corresponding to the at least two reference poses to calculate the initial angle of each joint.

[0120] Optionally, in the above technical solution, the data density of the correspondence table is configured according to the accuracy requirements of different areas in the robot's workspace.

[0121] Optionally, in the above technical solution, error compensation and singularity detection are performed on the initial angle of each joint to obtain the target angle of each joint. This includes: finding the error compensation value associated with the initial angle of each joint based on a pre-calibrated error mapping table, and adding the error compensation value of each joint to the initial angle to obtain the compensated angle of each joint; determining whether the compensated angle of each joint corresponds to the kinematic singular configuration of the robot, and obtaining the determination result; when the determination result is negative, the compensated angle of each joint is taken as the target angle of each joint; when the determination result is positive, the compensated angle of each joint is processed again through a preset singularity processing strategy, and the compensated angle of each joint is taken as the target angle of each joint.

[0122] It should be noted that the beneficial effects of the method for real-time forward and inverse kinematics calculation of a robot provided in the above embodiments are the same as the beneficial effects of the system for real-time forward and inverse kinematics calculation of a robot described above, and will not be repeated here. Furthermore, the system and method embodiments provided in the above embodiments belong to the same concept, and their specific implementation processes are detailed in the method embodiments, and will not be repeated here.

[0123] An electronic device according to an embodiment of the present invention includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements any of the above-mentioned methods for calculating forward and inverse kinematics of a robot in real time. The electronic device may also be a terminal device, which may be any device that can install applications, including at least one of smartphones, tablets, laptops, desktop computers, smart speakers, smartwatches, smart TVs, and smart in-vehicle devices.

[0124] An embodiment of the present invention provides a computer-readable storage medium storing a computer program, which, when executed by a processor, implements any of the above-described methods for real-time forward and inverse kinematics calculation of a robot.

[0125] The above description is merely a preferred embodiment of the present invention and an explanation of the technical principles employed. Those skilled in the art should understand that the scope of disclosure in this invention is not limited to technical solutions formed by specific combinations of the above-described technical features, but should also cover other technical solutions formed by arbitrary combinations of the above-described technical features or their equivalents without departing from the above-disclosed concept. For example, technical solutions formed by substituting the above features with (but not limited to) technical features with similar functions disclosed in this invention.

[0126] Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art can make changes, modifications, substitutions and variations to the above embodiments within the scope of the present invention.

Claims

1. A system for real-time kinematic forward and inverse solution computation for robots, characterized by, The data interface module, the forward solution calculation module, the inverse solution lookup table module and the error compensation module are included. The data interface module is configured to receive a target pose or preset angles of each joint sent by a master unit of a robot. The forward solution calculation module is configured to, when the preset angles of each joint are received, obtain an end effector pose of the robot according to the preset angles of each joint and through hardware parallel calculation. The inverse solution lookup table module is configured to, when the target pose is received, obtain initial angles of each joint corresponding to the target pose through a lookup table method. The error compensation module is configured to perform error compensation and singularity point detection on the initial angles of each joint to obtain target angles of each joint. The data interface module is further configured to send the end effector pose or the target angles of each joint to the master unit, so that the master unit controls the robot according to the end effector pose or the target angles of each joint. The inverse solution lookup table module is specifically configured to: query a pre-stored correspondence table between poses and joint angles according to the received target pose; if a same pose as the target pose is found in the correspondence table, directly take the angles of each joint corresponding to the same pose as initial angles; if no same pose as the target pose is found in the correspondence table, determine at least two reference poses closest to the target pose and reference angles of each joint corresponding to each reference pose, and perform interpolation operation on the reference angles of each joint corresponding to the at least two reference poses to calculate the initial angles of each joint.

2. A system for real-time forward and inverse kinematics solution of robots as claimed in claim 1 wherein, The data density of the correspondence table is configured according to accuracy requirements of different regions in a workspace of the robot.

3. A system for real-time forward and inverse kinematics solution of a robot according to claim 1 or 2, characterized in that, The error compensation module is configured to: based on a pre-calibrated error mapping table, find error compensation values associated with the initial angles of each joint, and add the error compensation values of each joint to the initial angles respectively to obtain compensated angles of each joint; determine whether the compensated angles of each joint correspond to a kinematic singularity configuration of the robot to obtain a determination result; when the determination result is no, take the compensated angles of each joint as target angles of each joint respectively; when the determination result is yes, perform reprocessing on the compensated angles of each joint through a preset singularity point processing strategy, and take the compensated angles of each joint as target angles of each joint respectively.

4. A method for real-time kinematic forward and inverse solution computation for robots, characterized in that, The data interface module is configured to receive a target pose or preset angles of each joint sent by a master unit of a robot. The forward solution calculation module is configured to, when the preset angles of each joint are received, obtain an end effector pose of the robot according to the preset angles of each joint and through hardware parallel calculation. The inverse solution lookup table module is configured to, when the target pose is received, obtain initial angles of each joint corresponding to the target pose through a lookup table method. The error compensation module is configured to perform error compensation and singularity point detection on the initial angles of each joint to obtain target angles of each joint. ​ send the end effector pose or the target angle of each joint to the master control unit, so that the master control unit controls the robot according to the end effector pose or the target angle of each joint; obtain the initial angle of each joint corresponding to the target pose by a table lookup method, comprising: query a pre-stored correspondence table between poses and joint angles according to the received target pose; if the same pose as the target pose is found in the correspondence table, then directly take the angle of each joint corresponding to the same pose as the initial angle; if the same pose as the target pose is not found in the correspondence table, then determine at least two reference poses closest to the target pose and the reference angle of each joint corresponding to each reference pose, and perform interpolation operation on the reference angle of each joint corresponding to at least two reference poses to calculate the initial angle of each joint.

5. A method for real-time forward and inverse kinematics solution of a robot according to claim 4, wherein, The data density of the correspondence table is configured according to the accuracy requirements of different regions in the workspace of the robot.

6. A method for real-time forward and inverse kinematics solution of a robot according to claim 4 or 5, characterized in that, perform error compensation and singularity detection on the initial angle of each joint to obtain the target angle of each joint, comprising: find the error compensation value associated with the initial angle of each joint based on a pre-calibrated error mapping table, and add the error compensation value of each joint to the initial angle to obtain the compensated angle of each joint; determine whether the compensated angle of each joint corresponds to the kinematic singularity configuration of the robot to obtain a determination result; when the determination result is no, then take the compensated angle of each joint as the target angle of each joint; when the determination result is yes, then perform reprocessing on the compensated angle of each joint through a pre-set singularity processing strategy, and take the compensated angle of each joint as the target angle of each joint.

7. An electronic device, comprising: a memory, a processor, and a computer program stored on the memory and executable on the processor, wherein the processor implements the method for real-time forward and inverse kinematics calculation of a robot according to any one of claims 4 to 6 when executing the computer program.

8. A computer-readable storage medium, characterized in that, The computer readable storage medium stores a computer program, and the computer program is executed by the processor to implement the method for real-time forward and inverse kinematics calculation of a robot according to any one of claims 4 to 6.

Images