A method and system for motion control of a robot arm

Abstract

The application discloses a kind of mechanical arm motion control method and system.The method includes: the real-time pose data of tracer under navigation equipment, object motion to target pose relative to reference object and the real-time force data of mechanical arm end are acquired;First control cycle, according to real-time pose data and target pose, the target pose data of mechanical arm end is calculated, and then the current end target motion parameter in the process of moving to target pose and current remaining motion time are calculated;Second control cycle, according to real-time force data and the real-time pose data of mechanical arm end, adjust end motion parameter, control mechanical arm end to move according to adjusted end motion parameter;When the current remaining motion time calculated by first control cycle is less than the minimum running time of set mechanical arm end, object moves to target pose.The application can realize that mechanical arm accurately and compliantly moves to specified pose.

Description

Technical Field

[0001] This invention belongs to the field of robot control technology, and more specifically, relates to a method and system for controlling the motion of a robotic arm. Background Technology

[0002] In numerous fields such as industrial production and medical surgery, robotic arms have been widely used due to their high precision and stability, becoming one of the core devices for automated operations. Currently, the most common control method for robotic arms in the industrial field is traditional position control. This control method can only precisely control the movement position of the robotic arm according to a preset path, but it cannot handle scenarios where the robotic arm interacts with the external environment. In such scenarios, when the robotic arm comes into contact with workpieces, obstacles, or other objects, forces are generated. Traditional position control lacks the ability to sense and adjust these forces, which can easily lead to robotic arm shutdowns and malfunctions. In severe cases, it can even cause damage to the robotic arm itself or the workpiece, making it difficult to meet the needs of complex operations.

[0003] As industries demand greater flexibility and adaptability from robotic arms, force-position control methods have become a research hotspot and have seen initial applications in industrial scenarios such as polishing and painting. Currently, the most widely used force-position control schemes in the industrial field primarily achieve precise control of the application of a specified force in a specific direction and the position in other directions by implementing force control and position control in different directions, thereby completing operations such as polishing and grinding of flat or curved surfaces. Among these, impedance control and admittance control are the most researched force-position control methods. Their core idea is to equate the interaction between the robotic arm and the external environment to a mass-damping-spring model, adjusting the robotic arm's motion state based on position difference (impedance control) or force difference (admittance control) to give the robotic arm a certain degree of operational compliance.

[0004] However, existing robotic arm force and position control methods still have significant limitations: on the one hand, they are difficult to adapt to complex mechanical environments. When the workpiece is covered or entangled by a flexible object, the force boundary is not smooth, or there is a potential risk of change in the working environment, existing methods cannot accurately perceive the dynamic changes in the mechanical properties of the environment, resulting in decreased control accuracy and insufficient operational stability. On the other hand, in the face of changes in the robotic arm's target pose caused by environmental changes, existing control methods lack the ability to respond quickly and adjust in real time, and cannot update control parameters in a timely manner to adapt to new operational requirements, thereby affecting operational efficiency and quality.

[0005] Therefore, there is an urgent need for a robotic arm motion control method that can adapt to complex mechanical environments and respond flexibly, to solve the problems of poor adaptability and untimely adjustment of existing technologies in complex scenarios, and to achieve compliant and precise motion control of robotic arms under complex conditions such as when obstacle boundaries cannot be clearly obtained, so as to further expand the application scenarios and operational capabilities of robotic arms.

[0006] The information disclosed in the background section of this invention is intended only to enhance the understanding of the general background of this invention, and should not be construed as an admission or in any way implying that such information constitutes prior art known to those skilled in the art. Summary of the Invention

[0007] The purpose of this invention is to propose a motion control method and system for robotic arms, which solves the technical problems of existing robotic arm force and position control methods being unable to adapt to complex mechanical environments and unable to adjust the target posture in a timely manner when the environment changes; and enables the robotic arm to move accurately and smoothly to the designated posture in complex mechanical environments, scenarios involving changes in the posture of related objects or its own base.

[0008] To achieve the above objectives, in a first aspect, the present invention proposes a method for controlling the motion of a robotic arm, comprising:

[0009] Acquire real-time pose data of the tracker under the navigation device, the target pose of the object fixed at the end of the robotic arm relative to the reference object, and the real-time force data of the end of the robotic arm;

[0010] In the first control cycle, the target pose data of the robotic arm end effector is calculated based on the real-time pose data and the target pose data, and then the current end effector target motion parameters and the current remaining motion time are calculated during the process of the robotic arm end effector moving the object to the target pose.

[0011] In the second control cycle, the end effector motion parameters are adjusted based on the real-time force data and the real-time pose data of the robotic arm end effector, and the robotic arm end effector is controlled to move according to the adjusted end effector motion parameters.

[0012] When the current remaining motion time calculated in the first control cycle is less than the set minimum running time of the robotic arm end effector, the first control cycle and the second control cycle end, the robotic arm end effector completes its motion, and the object moves to the target pose.

[0013] Optionally, the tracer includes:

[0014] A first tracer is fixedly connected to the end effector of the robotic arm, and the first tracer is used to acquire real-time pose data of the end effector of the robotic arm relative to the navigation device.

[0015] A second tracer is fixedly connected to the object and is used to acquire real-time pose data of the object relative to the navigation device.

[0016] A third tracer is fixedly connected to the reference object and is used to acquire the real-time pose data of the reference object relative to the navigation device.

[0017] Optionally, the real-time pose data includes:

[0018] The first real-time pose data is used to represent the real-time pose of the robotic arm end effector in the navigation device coordinate system;

[0019] The second real-time pose data is used to represent the real-time pose of the object in the coordinate system of the navigation device.

[0020] The third real-time pose data is used to represent the real-time pose of the reference object in the coordinate system of the navigation device.

[0021] The first real-time pose data, the second real-time pose data, and the third real-time pose data are all represented by a 4×4 matrix, denoted as . and

[0022] Optionally, within the first control cycle, the target pose of the robotic arm end-effector coordinate system relative to the navigation device is calculated based on the first real-time pose data, the second real-time pose data, the third real-time pose data, and the target pose data.

[0023] The calculation expression is:

[0024]

[0025] in, This is the inverse matrix of the second real-time pose data. The target pose data is given, and the end effector of the robotic arm is generally the center of the end effector flange, but it can also be defined by the user.

[0026] Optionally, it also includes:

[0027] Acquire the fourth real-time pose data;

[0028] The fourth real-time pose data is used to represent the real-time pose of the end effector coordinate system relative to the robot arm base coordinate system.

[0029] The fourth real-time pose data and the real-time force data at the end of the robotic arm are directly obtained through the control terminal of the robotic arm.

[0030] The fourth real-time pose data is represented by a 4×4 matrix, denoted as . The base of the robotic arm is the zero point of the base coordinate system.

[0031] Optionally, the pose data of the navigation device coordinate system relative to the robot arm base coordinate system is calculated based on the first real-time pose data and the fourth real-time pose data.

[0032] The calculation expression is:

[0033]

[0034] in, The inverse matrix of the first real-time pose data

[0035] Optionally, the target pose of the robotic arm end-effector coordinate system relative to the robotic arm base coordinate system is calculated based on the target pose of the robotic arm end-effector coordinate system relative to the navigation device and the pose data of the navigation device coordinate system relative to the robotic arm base coordinate system.

[0036] The calculation expression is:

[0037]

[0038] in, The target pose of the end effector coordinate system of the robotic arm relative to the base coordinate system of the robotic arm.

[0039] Optionally, the end effector motion parameters include:

[0040] The end target motion velocity includes the target motion velocity in six directions relative to the base coordinate system;

[0041] Based on the target pose data of the end-effector coordinate system relative to the base coordinate system and the fourth real-time pose data, the set maximum linear velocity of the robotic arm and the set maximum rotational velocity of the robotic arm, calculate the current end-effector target motion speed and the current remaining motion time required for the end-effector coordinate system to move to the target pose.

[0042] The calculation expression is:

[0043]

[0044] t = max(t) move , t rotation );

[0045]

[0046]

[0047] in, L is the transformation matrix from the current pose of the robotic arm's end effector coordinate system to the target pose relative to the robotic arm's base coordinate system. x0 L y0 L z0 L represents the component of the robot arm's end-effector displacement in the end-effector coordinate system. x L y L zLet θ be the component of the robot arm's end effector displacement in the base coordinate system. x0 θ y0 θ z0 Let θ be the component of the robot arm's end effector rotation in the end effector coordinate system. x θ y θ z This represents the component of the robotic arm's end-effector rotation in the base coordinate system; for The 3x3 rotation matrix in the matrix. for The 3x3 rotation matrix in the vector. The MatrixToRotationVector function can convert a rotation matrix into a rotation vector; V max V rmax The maximum linear speed and the maximum rotational speed of the robotic arm are set; t move and t rotation v represents the calculated translation and rotation times; x v y v z v rx v ry v rz The directional components of the current end-point target motion velocity required for the end-point coordinate system to move to the target pose.

[0048] Optionally, during the second control cycle, based on the fourth real-time pose data and the real-time force data of the robotic arm end effector, the velocity ratios of the robotic arm end effector in the six directions of the base coordinate system are calculated using the functions k = f(F) and k = f(M).

[0049] The speeds in the six directions of the current target motion speed are multiplied by the speed ratios of the corresponding directions to obtain the adjusted running speeds of the robotic arm end effector in the six directions, thereby controlling the robotic arm end effector to move at the adjusted running speeds.

[0050] The operating frequency of the first control cycle is less than the operating frequency of the second control cycle, and the operating interval of the first control cycle is:

[0051] t1=a*t(0 <a<1);

[0052] t2 <t1;

[0053] Where t1 is the running interval time of the first control cycle, t2 is the running interval time of the second control cycle, and t is the current remaining motion time;

[0054] The running interval of the second control cycle is set to a fixed value based on the command response time of the robotic arm.

[0055] Secondly, this invention proposes a robotic arm motion control system, comprising:

[0056] Control terminal, robotic arm, and navigation equipment;

[0057] The control terminal is used to acquire the real-time pose data of the navigation device and execute the robotic arm motion control method described in any one of the first aspects to control the robotic arm to move.

[0058] The beneficial effects of this invention are as follows: By acquiring real-time pose data from the tracer under the navigation device, target pose data, and real-time force data at the end of the robotic arm, the robotic arm motion control is achieved through a dual-control cycle collaborative operation, effectively solving the problems of poor adaptability and untimely adjustment in complex scenarios of existing technologies. The first control cycle accurately calculates the end-effector motion parameters and remaining motion time based on the real-time pose and target pose, providing a stable motion reference for the robotic arm; the second control cycle dynamically adjusts the motion parameters by combining real-time force and end-effector pose data, ensuring that the robotic arm maintains its motion compliance when interacting with external forces. The control process automatically ends when the remaining motion time is less than the set minimum running time, ensuring that the object accurately reaches the target pose. This control method not only enables the robotic arm to operate stably in complex mechanical environments such as workpieces covered by flexible objects and uneven force boundaries, but also flexibly adapts to dynamic scenarios such as changes in the pose of the reference object and changes in the pose of the robotic arm base, significantly improving the environmental adaptability, motion accuracy, and operational reliability of the robotic arm, and expanding its application scope in multiple fields such as industrial processing and medical surgery.

[0059] The system of the present invention has other features and advantages that will be apparent from or will be set forth in detail in the accompanying drawings and following detailed description, which together serve to explain the particular principles of the invention. Attached Figure Description

[0060] The above and other objects, features and advantages of the present invention will become more apparent from the accompanying drawings, in which like reference numerals generally denote like parts.

[0061] Figure 1 A flowchart illustrating the steps of a robotic arm motion control method according to Embodiment 1 of the present invention is shown.

[0062] Figure 2 A flowchart illustrating the steps of a robotic arm motion control method according to Embodiment 2 of the present invention is shown.

[0063] Figure 3A flowchart of the first control cycle according to Embodiment 2 of the present invention is shown.

[0064] Figure 4 A flowchart of the second control cycle according to Embodiment 2 of the present invention is shown.

[0065] Figure 5 a and Figure 5 b shows schematic diagrams illustrating the relationship between the force / torque and the speed proportionality coefficient of the robotic arm according to Embodiment 2 of the present invention. Detailed Implementation

[0066] The invention will now be described in more detail with reference to the accompanying drawings. While preferred embodiments of the invention are shown in the drawings, it should be understood that the invention can be implemented in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided so that the invention will be thorough and complete, and will fully convey the scope of the invention to those skilled in the art.

[0067] Example 1

[0068] like Figure 1 As shown, this embodiment provides a robotic arm motion control method, including:

[0069] Acquire real-time pose data of the tracker under the navigation device, the target pose of the object fixed at the end of the robotic arm relative to the reference object, and real-time force data of the end of the robotic arm;

[0070] In the first control cycle, the target pose data of the robotic arm end effector is calculated based on the real-time pose data and the target pose data. Then, the current end effector target motion parameters and the current remaining motion time are calculated during the process of the robotic arm end effector moving the object to the target pose.

[0071] In the second control cycle, the end effector motion parameters are adjusted based on real-time force data and real-time pose data of the robotic arm end effector, and the robotic arm end effector moves according to the adjusted end effector motion parameters.

[0072] When the current remaining motion time calculated in the first control cycle is less than the set minimum running time of the robotic arm end effector, the first and second control cycles end, the robotic arm end effector completes its motion, and the object moves to the target pose.

[0073] Specifically, this robotic arm motion control method uses "real-time multi-data acquisition - dual-cycle collaborative control - precise termination judgment" as its core logic to achieve compliant motion control of an object towards a target pose of a reference object in complex scenarios. The specific process is as follows: First, multi-dimensional data acquisition is completed through navigation devices and sensors. The navigation device acquires the pose data of the tracers (fixed at the end of the robotic arm, the object to be moved, and the reference object) in their coordinate system in real time, accurately capturing the spatial position and attitude relationship of the three. At the same time, the force sensor mounted on the end of the robotic arm acquires the force data of the end in the tool coordinate system in real time, including the component force (F) in the XYZ directions. x F y F z ) and component torque (M x M y M z In addition, a complete control data input system is constructed by combining the preset "target pose of the object to be moved relative to the reference object".

[0074] In the control execution phase, a dual-cycle collaborative working mode is adopted: The first control cycle is centered on "pose planning". Based on the real-time pose data of the tracker and the preset target pose, the target pose data of the robotic arm end effector is determined through coordinate transformation and kinematic calculation: First, based on the fixed pose relationship between the object to be moved and the tracker, and the reference object and the tracker, the target pose of the tracker is derived. Then, combined with the calibration relationship between the tracker and the robotic arm tool coordinate system (fixed values ​​obtained from hand-eye calibration) and the pose of the current tool coordinate system relative to the base, the target pose of the robotic arm tool coordinate system is finally obtained. Subsequently, based on the set maximum linear velocity (e.g., 2 mm / s) and maximum rotational velocity (e.g., 0.00873 rad / s), the current end effector target motion parameters (including the translational velocity v in the XYZ direction under the base coordinate system) required for the robotic arm end effector to move the object to the target pose are calculated. x v y v z With rotational speed v around the axis rx v ry v rz ), and the current remaining motion time t.

[0075] The second control cycle, centered on "force control adjustment," has a significantly higher frequency than the first control cycle (e.g., a typical value of 2ms / cycle, compared to 2s / cycle for the first control cycle), achieving dynamic adaptive adjustment: First, based on the current pose data of the robotic arm's end effector, the force data collected by the force sensor is transformed and decomposed from the tool coordinate system to the base coordinate system, yielding the component force (F) in the base coordinate system. xb F yb F zb ) and component torque (M xb M ybM zb Then, using the preset velocity scaling functions k = f(F) (based on force data) and k = f(M) (based on torque data), the velocity scaling coefficients (k) in the six directions of the base coordinate system are calculated. x k y k z k rx k ry k rz The end target motion parameters obtained in the first control cycle are multiplied by the velocity proportional coefficient in the corresponding direction to obtain the adjusted end target velocity. The end of the robotic arm is driven to move at this velocity to ensure that the speed can be reduced in real time when the force is abnormal (such as excessive contact force) to avoid damage to the robotic arm or object.

[0076] During the control termination judgment phase, the current remaining motion time t calculated in the first control cycle is continuously monitored. When t is less than the set minimum running time t of the robotic arm end effector, the termination judgment is made. min At a time interval of 0.25s, it is determined that the end effector of the robotic arm has approached and reached the target pose. At this point, the first control cycle and the second control cycle end synchronously, the robotic arm stops moving, and the entire control process is completed once the moving object accurately reaches the target pose relative to the reference object. This method, through the synergy of "low-frequency pose planning + high-frequency force control adjustment" in a dual-cycle manner, ensures both the accuracy of the motion path and the dynamic adaptability to complex mechanical environments, effectively handling scenarios such as changes in the pose of the reference object and variations in the pose of the robotic arm base.

[0077] In this embodiment, the tracer includes:

[0078] The first tracer is fixedly connected to the end effector of the robotic arm and is used to acquire real-time pose data of the end effector of the robotic arm relative to the navigation device.

[0079] The second tracer is fixedly connected to the object and is used to acquire the object's real-time pose data relative to the navigation device.

[0080] The third tracer is fixedly connected to the reference object and is used to acquire the real-time pose data of the reference object relative to the navigation device.

[0081] Specifically, in this embodiment, the tracker adopts a three-component independent configuration design. Through collaborative work with navigation devices (such as optical trackers), a full-dimensional pose perception system is constructed for the robotic arm, the object to be moved, and the reference object. Each tracker has precise spatial positioning capabilities and a clear division of labor: the first tracker is rigidly connected to the flange or designated mounting surface at the end of the robotic arm. Its body integrates multiple retroreflective markers (reflective sheets or marker balls) that can reflect the infrared light emitted by the navigation device. The navigation device receives the reflected light and calculates it through triangulation algorithms and coordinate transformations, outputting in real time the three-dimensional position (X, Y, Z coordinates) and attitude (rotation angle around the three axes) data of the robotic arm end in the global coordinate system of the navigation device, providing an end-effector reference for the robotic arm motion control. The second tracker is fixedly connected to the surface of the object to be moved by the robotic arm. The connection method is adapted to the object's shape characteristics to ensure no relative displacement. Its working principle is the same as the first tracker. Through real-time tracking by the navigation device, it continuously acquires the dynamic pose data of the object relative to the navigation device's coordinate system, accurately capturing the object's position and attitude changes during movement, providing a direct basis for determining whether the object has reached the target state. The third tracker is fixed in the stable area of ​​a reference object, which can be a fixed reference part or a target-related part in the work scene. This tracker acquires the real-time pose data of the reference object in the global coordinate system through the navigation device, forming a stable position and attitude reference, providing a core reference for calculating the relative pose of the object to be moved relative to the reference object and determining the target motion parameters. The three trackers work together to achieve comprehensive acquisition and real-time synchronization of pose data between the robotic arm, the object to be moved, and the reference object, laying a precise data foundation for the implementation of dual-cycle control logic.

[0082] In this embodiment, the real-time pose data includes:

[0083] The first real-time pose data is used to represent the real-time pose of the robotic arm end effector in the coordinate system of the navigation device.

[0084] The second real-time pose data is used to represent the real-time pose of the object in the coordinate system of the navigation device.

[0085] The third real-time pose data is used to represent the real-time pose of the reference object in the coordinate system of the navigation device.

[0086] The first real-time pose data, the second real-time pose data, and the third real-time pose data are all represented by a 4×4 matrix, denoted as . and

[0087] Specifically, in this embodiment, the real-time pose data is constructed around the spatial state monitoring of the robotic arm, the object to be moved, and the reference object. It is clearly divided into three types of targeted data, all of which are represented using a unified 4×4 homogeneous transformation matrix format to ensure the consistency and accuracy of data transmission and calculation: Among them, the first real-time pose data (denoted as...) The core is used to accurately describe the real-time spatial state of the robotic arm's end effector in the navigation device's coordinate system. This matrix contains both the three-dimensional translational coordinate information (position offset in the X, Y, and Z axes) of the robotic arm's end effector relative to the origin of the navigation device's coordinate system, and the rotational attitude information of the robotic arm's end effector around the three-dimensional coordinate axes (represented by 3×3 rotation matrix components), completely reflecting the spatial position and attitude relationship of the robotic arm's end effector; the second real-time pose data (denoted as...) The third real-time pose data (denoted as ) is specifically used to characterize the real-time pose of a moving object in the coordinate system of the navigation device. Its matrix structure is consistent with the first real-time pose data. Through the combination of translation and rotation components, it captures the position changes and attitude adjustments of the object relative to the navigation device in real time during motion, providing dynamic data support for subsequent target pose calibration. The system focuses on the real-time spatial state of the reference object in the navigation device's coordinate system. It also uses a 4×4 homogeneous transformation matrix to store the three-dimensional position coordinates and rotational attitude parameters of the reference object, forming a stable spatial reference. All three types of data are represented in matrix form, directly adapting to the coordinate transformation and relative pose calculation algorithms in robotic arm motion control without requiring additional data format conversion. This significantly improves the computational efficiency of the control process. Furthermore, clear symbol definitions (the superscript "nav" identifies the navigation device's coordinate system, and the subscripts distinguish between the robotic arm's end effector, the object, and the reference object) make data attribution and application scenarios immediately clear, providing standardized and high-precision data input for core aspects such as target pose derivation and motion parameter calculation in the dual control cycle.

[0088] In this embodiment, within the first control cycle, the target pose of the robotic arm end effector coordinate system relative to the navigation device is calculated based on the first real-time pose data, the second real-time pose data, the third real-time pose data, and the target pose data.

[0089] The calculation expression is:

[0090]

[0091] in, This is the inverse matrix of the second real-time pose data. The target pose data is typically the center of the end effector flange of the robotic arm, but it can also be defined by the user.

[0092] Specifically, the core computational task of the first control cycle is to accurately deduce the target pose that the robotic arm's end effector should achieve in the navigation device's coordinate system, based on the real-time perception data provided by the navigation system and the preset target relationship. This computational process essentially involves constructing a complete coordinate transformation chain, whose logical starting point is the known real-time relationship between the robotic arm's end effector and the navigation coordinate system (i.e., the relationship provided by the first tracer). The endpoint is to deduce the future target position in the navigation coordinate system. The key to the calculation lies in embedding the constraint of "target pose of the manipulated object relative to the reference object" into this chain.

[0093] First, data from the second tracer is used. inverse matrix Its physical meaning is to transform a point or vector from the navigation coordinate system to the coordinate system of the object being manipulated; then, multiply it by the pre-set target pose data. This represents the position and orientation of the object we want to manipulate in the reference object's coordinate system; then, it is multiplied by the real-time pose of the reference object in the navigation coordinate system provided by the third tracer. Through this series of matrix multiplications The system then maps the current pose of the robotic arm's end effector to the navigation coordinate system via two intermediate frames: the "operating object coordinate system" and the "reference object coordinate system." This allows the system to determine the target pose that the robotic arm's end effector must reach in global space to satisfy the relative poses between objects. Here, "robotic arm end effector" typically refers to the center point of its flange, but it can also be customized as other effective tool control points depending on the actual tool connection configuration.

[0094] In this embodiment, it also includes:

[0095] Acquire the fourth real-time pose data;

[0096] The fourth real-time pose data is used to represent the real-time pose of the end effector coordinate system relative to the robot arm base coordinate system;

[0097] The fourth real-time pose data and the real-time force data at the end of the robotic arm can be directly obtained through the control terminal of the robotic arm.

[0098] The fourth real-time pose data is represented by a 4×4 matrix, denoted as . The base of the robotic arm is the zero point of the base coordinate system.

[0099] Specifically, the fourth real-time pose data represents the real-time position and orientation of the robotic arm's end effector coordinate system (usually defined as the flange center or tool center point) relative to the robotic arm's base coordinate system. It is a standard 4×4 homogeneous transformation matrix, denoted as... This matrix contains both the three-dimensional translational position information of the end effector coordinate system relative to the base coordinate system (i.e., coordinate offsets along the X, Y, and Z axes) and the rotational attitude information of the end effector coordinate system about the three-dimensional coordinate axes (represented by the 3×3 rotation components in the matrix), which can completely and accurately reflect the dynamic spatial state of the robotic arm's end effector relative to its base. To ensure the real-time performance and reliability of data acquisition, the fourth real-time pose data and the real-time force data of the robotic arm's end effector are directly collected through the control end of the robotic arm, without the need for additional intermediate transmission modules, effectively reducing data latency and transmission errors. The real-time force data includes the XYZ direction force components of the end effector in the tool coordinate system (F...). x F y F z ) and component torque (M x M y M z This provides direct force feedback for speed adjustment in the second control cycle. The base of the robotic arm serves as the zero point of the base coordinate system. This definition provides a stable and unified reference for the calculation of the fourth real-time pose data, ensuring the consistency and accuracy of the pose data. It also provides crucial coordinate reference support for subsequently converting the target pose in the navigation device coordinate system into motion parameters in the base coordinate system and achieving precise control of the robotic arm.

[0100] In this embodiment, the pose data of the navigation device coordinate system relative to the robot arm base coordinate system is calculated based on the first real-time pose data and the fourth real-time pose data.

[0101] The calculation expression is:

[0102]

[0103] in, is the inverse matrix of the first real-time pose data.

[0104] Specifically, the pose data of the navigation device coordinate system relative to the robotic arm base coordinate system is precisely derived through matrix operations of the first real-time pose data and the fourth real-time pose data. The core calculation expression is: Its computational logic is rigorous and adaptable to the coordinate transformation requirements of robotic arm control: among which The fourth real-time pose data is a 4×4 homogeneous transformation matrix of the robot arm's end-effector coordinate system relative to the base coordinate system. It fully contains the translation and rotation information of the end-effector relative to the base and serves as the basic reference data for calculation. It is the first real-time pose data The inverse matrix of the first real-time pose data originally represents the pose of the robotic arm's end effector in the navigation device's coordinate system. Through inversion, the coordinate transformation direction can be reversed, that is, the pose relationship from "end effector relative to navigation device" can be transformed into the pose relationship from "navigation device relative to end effector". This is achieved by using the matrix representing "end effector relative to base". Compared with characterizing "navigation device relative to end" By performing matrix multiplication, the spatial pose relationships of the base, end effector, and navigation device can be sequentially linked together, ultimately yielding... This 4×4 homogeneous transformation matrix accurately describes the three-dimensional translational position and three-dimensional rotational attitude of the navigation device coordinate system relative to the robot arm base coordinate system. The core value of this calculation process lies in establishing a unified relationship between the two coordinate systems, which is crucial for subsequently determining the pose of the robot arm's end effector in the navigation device coordinate system. Converting motion parameters into the base coordinate system provides a crucial bridge, ensuring seamless connection of pose data in different coordinate systems and guaranteeing the accuracy and logical consistency of robotic arm motion control.

[0105] In this embodiment, the target pose of the robotic arm end-effector coordinate system relative to the robotic arm base coordinate system is calculated based on the target pose of the robotic arm end-effector coordinate system relative to the navigation device and the pose data of the navigation device coordinate system relative to the robotic arm base coordinate system.

[0106] The calculation expression is:

[0107]

[0108] in, The target pose is defined by the coordinate system of the robotic arm's end effector relative to the coordinate system of the robotic arm's base.

[0109] Specifically, the target pose of the robotic arm's end effector coordinate system relative to the robotic arm's base coordinate system is derived through matrix collaborative operations using the two types of core pose data that have already been calculated. The core calculation expression is as follows: This computational logic aims to achieve precise conversion and connection of target poses in different coordinate systems: where It is the 4×4 homogeneous transformation matrix of the navigation device coordinate system relative to the robotic arm base coordinate system obtained earlier. It fully carries the three-dimensional translation position and three-dimensional rotation attitude relationship between the navigation device and the base coordinate system, and provides a stable reference bridge for coordinate transformation. This refers to the target pose matrix calculated in the first control cycle, where the robot arm's end effector coordinate system is relative to the navigation device's coordinate system. This clearly defines the ideal spatial state the end effector needs to reach in the navigation device's global coordinate system. By representing "the navigation device relative to the base..." With respect to the characterization of "endpoint relative to navigation device" By performing matrix multiplication, the transitivity of matrix operations can be used to connect the spatial pose relationships of the base, navigation equipment, and robotic arm end effector, ultimately yielding... This 4×4 homogeneous transformation matrix accurately represents the target pose of the robotic arm's end effector coordinate system relative to its base coordinate system. It includes both the target translation coordinates relative to the base and the target rotational attitude, successfully transforming the abstract target pose in the navigation device coordinate system into specific target parameters in the base coordinate system that the robotic arm control end can directly recognize and execute. This provides core target reference data for subsequent calculations of the end effector's motion speed and the achievement of precise motion control, ensuring the consistency of control logic and the accuracy of motion execution.

[0110] In this embodiment, the end effector motion parameters include:

[0111] The end-point target velocity includes the target velocity in six directions relative to the base coordinate system;

[0112] Based on the target pose data of the end-effector coordinate system relative to the base coordinate system and the fourth real-time pose data, the set maximum linear speed of the robotic arm and the set maximum rotational speed of the robotic arm, calculate the current end-effector target motion speed and the current remaining motion time required for the end-effector coordinate system to move to the target pose.

[0113] The calculation expression is:

[0114]

[0115]

[0116] t = max(t) move , t rotation );

[0117]

[0118] in, L is the transformation matrix from the current pose of the robot arm's end effector coordinate system relative to the robot arm's base coordinate system to the target pose. x0 L y0 L z0 L represents the component of the robot arm's end-effector displacement in the end-effector coordinate system. x L y L z Let θ be the component of the robot arm's end effector displacement in the base coordinate system. x0 θ y0 θ z0 Let θ be the component of the robot arm's end effector rotation in the end effector coordinate system. x θ y θ zThis represents the component of the robotic arm's end-effector rotation in the base coordinate system; for The 3x3 rotation matrix in the matrix. for The 3x3 rotation matrix; the MatrixToRotationVector function can convert a rotation matrix into a rotation vector; V max V rmax The maximum linear speed and maximum rotational speed of the robotic arm are set; t move and t rotation v represents the calculated translation and rotation times; x v y v z v rx v ry v rz The directional components of the current end-point target motion velocity required for the end-point coordinate system to move to the target pose.

[0119] Specifically, the core of the end-effector motion parameters is the end-effector target motion velocity. This velocity is specifically defined as the target motion velocity in six directions relative to the base coordinate system of the robotic arm's end-effector coordinate system. It encompasses the translational target velocity along the X, Y, and Z axes and the rotational target velocity around the X, Y, and Z axes, comprehensively covering all dimensions of the robotic arm's end-effector spatial motion. To accurately obtain this end-effector target motion velocity and the corresponding remaining motion time, three types of key data are required for calculation: first, the derived target pose data of the end-effector coordinate system relative to the base coordinate system. The first is to clearly define the ideal spatial state that the end effector needs to reach; the second is to collect real-time pose data. The calculation process involves three key parameters: first, the actual pose of the end effector relative to the base coordinate system; second, the pre-set maximum linear and rotational speeds of the robotic arm, which define safe and efficient boundary thresholds for motion speed. The calculations are performed by first using the target pose data and the fourth real-time pose data, then employing a coordinate transformation algorithm to obtain the spatial displacement (translational distance along the three axes) and attitude change (rotation angle around the three axes) of the end effector from its current pose to the target pose. Next, combining the set maximum linear and rotational speeds, the theoretical time required to complete the corresponding displacement and attitude change in the translation and rotational directions is calculated, and the larger of the two values ​​is taken as the remaining motion time. Finally, based on the matching relationship between displacement, attitude change, and remaining motion time, the target motion speed in six directions is distributed to ensure that the end effector can gradually approach and reach the target pose in a smooth and precise manner, providing stable basic motion parameters for the force control adjustment in the subsequent second control cycle.

[0120] In this embodiment, during the second control cycle, based on the fourth real-time pose data and the real-time force data of the robotic arm end effector, the velocity ratios of the robotic arm end effector in six directions in the base coordinate system are calculated using the functions k = f(F) and k = f(M).

[0121] The speed of the current target motion is multiplied by the speed ratio of the six directions to obtain the adjusted running speed of the robotic arm end effector in the six directions, and then the robotic arm end effector is controlled to move at the adjusted running speed.

[0122] The operating frequency of the first control cycle is less than that of the second control cycle, and the operating interval of the first control cycle is:

[0123] t1=a*t(0 <a<1);

[0124] t2 <t1;

[0125] Where t1 is the running interval time of the first control cycle, t2 is the running interval time of the second control cycle, and t is the current remaining motion time;

[0126] The running interval of the second control cycle is set to a fixed value based on the command response time of the robotic arm.

[0127] Specifically, in the second control cycle, the motion adjustment of the robotic arm's end effector is centered on real-time force feedback and pose data. It adapts to complex mechanical environments through dynamic speed ratios, while employing a cycle design of "low-frequency planning + high-frequency adjustment" to ensure control accuracy and timely response: firstly, it combines the fourth real-time pose data ( Real-time pose of the end effector relative to the base coordinate system and real-time force data of the robot end effector (including the XYZ component force F in the tool coordinate system). x F y F z and component torque M x M y M z First, the force data is transformed from the tool coordinate system to the base coordinate system. Then, using the preset velocity scaling functions k = f(F) (based on force data) and k = f(M) (based on torque data), the velocity scaling coefficients (k) in the six directions of translation along the X, Y, and Z axes and rotation around the X, Y, and Z axes in the base coordinate system are calculated respectively. x k y k z k rx k ry k rz This proportional coefficient dynamically changes with the magnitude of the applied force / torque, achieving adaptive matching between force control and velocity. Subsequently, the velocities (v) in six directions from the current target motion velocity calculated in the first control cycle are... x, v y , v z , v rx , v ry , v rz ), are multiplied by the velocity proportionality coefficients in the corresponding directions to obtain the adjusted operating speeds in six directions. The control end of the robotic arm drives the movement of the end according to the adjusted speeds, ensuring that the movement speed can be reduced in real time when the force is abnormal (such as excessive contact force), avoiding damage to the robotic arm or the operating object, and ensuring the movement compliance. In the design of the periodic operating parameters, the operating frequency of the first control period is significantly less than that of the second control period. The operating interval time t1 of the first control period is dynamically set by the formula t1 = a * t (0 < a < 1), where t is the current remaining movement time calculated for the first control period. The value range of the proportionality coefficient a ensures that t1 is always less than the remaining movement time, enabling the first control period to continuously update the target movement parameters and avoiding control lag. The operating interval time t2 of the second control period satisfies t2 < t1, and t2 is set to a fixed value according to the command response time of the robotic arm. Through high-frequency real-time adjustment, the change in the force at the end is quickly responded to, realizing the collaborative working mode of "low-frequency pose planning + high-frequency force control adjustment", which not only ensures the stability of the movement path but also improves the dynamic adaptability of the robotic arm to complex environments.

[0128] Embodiment 2

[0129] As Figure 2 shown, this embodiment provides a method for controlling the movement of a robotic arm, including:

[0130] The robotic arm has 6 or more degrees of freedom, and the control end can obtain the pose T of the tool coordinate system of the robotic arm end (center of the flange) relative to the base coordinate system in real time tool2Base (position and orientation, represented by a 4x4 matrix). A six-axis force sensor is fixed at the end of the robotic arm, and the control end can obtain the force condition at the end of the robotic arm in real time, which is represented as F in the tool coordinate system of the robotic arm end (center of the flange) x , F y , F z , M x , M y , M z , which are the component forces and component torques in the X-axis, Y-axis, and Z-axis directions respectively;

[0131] A tracer 1 is fixed to the end of a robotic arm. Object A is fixedly connected to the end of the robotic arm via a connecting component. A tracer 2 is fixed to object A, and a tracer 3 is fixed to object B. The tracking device acquires the pose (position and attitude, represented by a 4x4 matrix) data of tracers 1, 2, and 3 in the tracking device coordinate system in real time, denoted as T1, T2, and T3. The robotic arm motion control method in this embodiment consists of a first control cycle and a second control cycle. The first control cycle calculates the end velocity and movement time of the robotic arm end to the target pose based on the navigation device data and the end pose information of the robotic arm end. The second control cycle adjusts the end velocity calculated in the first control cycle based on the force on the end of the robotic arm end and controls the movement of the robotic arm.

[0132] The process of the first control cycle is as follows: Figure 3 As shown, in order to move object A to a pose T relative to object B... A2B According to the pose T of object A relative to tracer 2 A22 The pose T of object B relative to tracer 3 B23 The target pose T of tracer 2 is calculated. 2t .

[0133]

[0134] Then, based on the current pose T of tracer 1 relative to tracer 2 122 Calculate the target pose T of tracer 1 1t .

[0135]

[0136] T 1t =T 2t ×T 122 ;

[0137] Then, based on the pose T of tracer 1 relative to the tool coordinate system of the robotic arm... 12tool (Obtained through hand-eye calibration, fixed value), current tool coordinate system pose T in robot arm base coordinate system. tool2Base Calculate the target pose T in the tool coordinate system of the robotic arm. tool2Baset .

[0138]

[0139] According to T tool2Base and T tool2Baset And the set maximum linear speed V max (Typical value 2 mm / s) and maximum rotational speed V rmax(Typical value 0.00873 rad / s), calculate the ideal time t for the robotic arm's movement, and then calculate the 6-axis velocity v in the robotic arm base coordinate system that moves the robotic arm tool coordinate system to the target pose. x v y v z v rx v ry v rz (v represents translation along the axis, v) r (For rotation about the axis). The specific calculation process is as follows: based on T tool2Base and T tool2Baset Calculate the transformation relationship T from the target pose in the robot arm tool coordinate system to the current pose in the robot arm worker coordinate system. toolt2tool , will T toolt2tool Decomposed into displacement L in the current tool coordinate system x0 L y0 L z0 and rotation vector Lr x0 L ry0 L rz0 Then, according to the coordinate system of the robotic arm base, it is decomposed again into displacement L. x L y L z and rotation vector L rx L ry L rz According to V max and V rmax Calculate the ideal motion time t and v of the robotic arm. x v y v z v rx v ry v rz The formula is as follows, where the symbol R represents a 3x3 rotation matrix.

[0140]

[0141] R toolt2tool =RotationMatrix(T) toolt2tool );

[0142]

[0143] R tool2Base =RotationMatrix(T) tool2Base );

[0144] t = max(t1, t2);

[0145]

[0146] The process of the second control cycle is as follows Figure 4 shown, obtain the current force data F at the end of the robotic arm x , F y , F z , M x , M y , M z , and according to the current end pose T of the robotic arm tool2Base , decompose the force data into F again in the direction of the robotic arm base coordinate system xb , F yb , F zb , M xb , M yb , M zb , then calculate the speed ratios k in each direction according to the functions k = f(F) and k = f(M) x , k y , k z , k rx , k ry , k rz , multiply the speeds in each direction calculated in the first control cycle by the speed ratios to obtain the target speed at the end of the robotic arm, and control the execution of the end of the robotic arm.

[0147] The speed ratio function curve is as follows Figure 5 shown, the speed ratio functions f(F) and f(M) can be adjusted according to the specific scenario, and the typical formulas are

[0148]

[0149] The first control cycle is executed at a low frequency, once every t1 time (typical value 2s), and the second control cycle is executed at a high frequency, once every t2 (typical value 2ms) time. t1 is affected by the ideal motion time t of the robotic arm calculated in the first control cycle, t1 = a * t (0 < a < 1), and t2 is related to the command response time of the robotic arm and is usually a fixed value. When t is less than the set minimum running time tmin (typical value 0.25s), it is considered that the end of the robotic arm reaches the target pose and the entire control ends.

[0150] Embodiment 3

[0151] This embodiment provides a robotic arm motion control system, including

[0152] a control end, a robotic arm, and a navigation device;

[0153] The control end is used to obtain the real-time pose data of the navigation device and execute the robotic arm motion control method described in Embodiment 1 to control the motion of the robotic arm.

[0154] The various embodiments of the present invention have been described above. These descriptions are exemplary and not exhaustive, nor are they limited to the disclosed embodiments. Many modifications and variations will be apparent to those skilled in the art without departing from the scope and spirit of the described embodiments.

Claims

1. A method for controlling the motion of a robotic arm, characterized in that, include: Acquire real-time pose data of the tracker under the navigation device, the target pose of the object fixed at the end of the robotic arm relative to the reference object, and the real-time force data of the end of the robotic arm; In the first control cycle, the target pose data of the robotic arm end effector is calculated based on the real-time pose data and the target pose data, and then the current end effector target motion parameters and the current remaining motion time are calculated during the process of the robotic arm end effector moving the object to the target pose. In the second control cycle, the end effector motion parameters are adjusted based on the real-time force data and the real-time pose data of the robotic arm end effector, and the robotic arm end effector is controlled to move according to the adjusted end effector motion parameters. When the current remaining motion time calculated in the first control cycle is less than the set minimum running time of the robotic arm end effector, the first control cycle and the second control cycle end, the robotic arm end effector completes its motion, and the object moves to the target pose.

2. The robotic arm motion control method according to claim 1, characterized in that, The tracer includes: A first tracer is fixedly connected to the end effector of the robotic arm, and the first tracer is used to acquire real-time pose data of the end effector of the robotic arm relative to the navigation device. A second tracer is fixedly connected to the object and is used to acquire real-time pose data of the object relative to the navigation device. A third tracer is fixedly connected to the reference object and is used to acquire the real-time pose data of the reference object relative to the navigation device.

3. The robotic arm motion control method according to claim 2, characterized in that, The real-time pose data includes: The first real-time pose data is used to represent the real-time pose of the robotic arm end effector in the navigation device coordinate system; The second real-time pose data is used to represent the real-time pose of the object in the coordinate system of the navigation device. The third real-time pose data is used to represent the real-time pose of the reference object in the coordinate system of the navigation device. The first real-time pose data, the second real-time pose data, and the third real-time pose data are all represented by a 4×4 matrix, denoted as . and 4. The robotic arm motion control method according to claim 3, characterized in that, Within the first control cycle, the target pose of the robotic arm end coordinate system relative to the navigation device is calculated based on the first real-time pose data, the second real-time pose data, the third real-time pose data, and the target pose data. The calculation expression is: in, This is the inverse matrix of the second real-time pose data. The data refers to the target pose. The end effector of the robotic arm is generally the center of the end effector flange, but it can also be defined by the user.

5. The robotic arm motion control method according to claim 4, characterized in that, Also includes: Acquire the fourth real-time pose data; The fourth real-time pose data is used to represent the real-time pose of the end effector coordinate system relative to the robot arm base coordinate system. The fourth real-time pose data and the real-time force data at the end of the robotic arm are directly obtained through the control terminal of the robotic arm. The fourth real-time pose data is represented by a 4×4 matrix, denoted as . The base of the robotic arm is the zero point of the base coordinate system.

6. The robotic arm motion control method according to claim 5, characterized in that, Based on the first real-time pose data and the fourth real-time pose data, calculate the pose data of the navigation device coordinate system relative to the robot arm base coordinate system; The calculation expression is: in, is the inverse matrix of the first real-time pose data.

7. The robotic arm motion control method according to claim 6, characterized in that, Based on the target pose of the robotic arm end-effector coordinate system relative to the navigation device and the pose data of the navigation device coordinate system relative to the robotic arm base coordinate system, the target pose of the robotic arm end-effector coordinate system relative to the robotic arm base coordinate system is calculated. The calculation expression is: in, The target pose of the end effector coordinate system of the robotic arm relative to the base coordinate system of the robotic arm.

8. The robotic arm motion control method according to claim 7, characterized in that, The end-effector motion parameters include: The end target motion velocity includes the target motion velocity in six directions relative to the base coordinate system; Based on the target pose data of the end-effector coordinate system relative to the base coordinate system and the fourth real-time pose data, the set maximum linear velocity of the robotic arm and the set maximum rotational velocity of the robotic arm, calculate the current end-effector target motion speed and the current remaining motion time required for the end-effector coordinate system to move to the target pose. The calculation expression is: t=max(t move ,t rotation ); in, L is the transformation matrix from the current pose of the robotic arm's end effector coordinate system to the target pose relative to the robotic arm's base coordinate system. x0 L y0 L z0 L represents the component of the robot arm's end-effector displacement in the end-effector coordinate system. x L y L z Let θ be the component of the robot arm's end effector displacement in the base coordinate system. x0 θ y0 θ z0 Let θ be the component of the robot arm's end effector rotation in the end effector coordinate system. x θ y θ z This represents the component of the robotic arm's end-effector rotation in the base coordinate system; for The 3x3 rotation matrix in the matrix. for The 3x3 rotation matrix; the MatrixToRotationVector function can convert a rotation matrix into a rotation vector; V max V rmax The maximum linear speed and the maximum rotational speed of the robotic arm are set; t move and t rotation v represents the calculated translation and rotation times; x v y v z v rx v ry v rz The directional components of the current end-point target motion velocity required for the end-point coordinate system to move to the target pose.

9. The robotic arm motion control method according to claim 8, characterized in that, In the second control cycle, based on the fourth real-time pose data and the real-time force data of the end effector of the robotic arm, the velocity ratios of the end effector of the robotic arm in the six directions in the base coordinate system are calculated using the functions k = f(F) and k = f(M). The speeds in the six directions of the current target motion speed are multiplied by the speed ratios of the corresponding directions to obtain the adjusted running speeds of the robotic arm end effector in the six directions, thereby controlling the robotic arm end effector to move at the adjusted running speeds. The operating frequency of the first control cycle is less than the operating frequency of the second control cycle, and the operating interval of the first control cycle is: t1=a*t(0 <a<1); t2 <t1; Where t1 is the running interval time of the first control cycle, t2 is the running interval time of the second control cycle, and t is the current remaining motion time; The running interval of the second control cycle is set to a fixed value based on the command response time of the robotic arm.

10. A motion control system for a robotic arm, characterized in that, include: Control terminal, robotic arm, and navigation equipment; The control terminal is used to acquire the real-time pose data of the navigation device and execute the robotic arm motion control method according to any one of claims 1-9 to control the robotic arm to move.

Images