Mechanical arm kinematics modeling and motion control method, device and system

Abstract

The invention discloses a mechanical arm kinematics modeling and motion control method, device and system. The mechanical arm comprises N joints connected in series and a sand disc connected to the tail end, and a radial distance R exists between the working edge of the sand disc and the tail end arm section. The kinematics equation construction method comprises the following steps: determining a working edge reference point of a sand disc as a motion control target; based on mechanical structure parameters, joint variables and a radial distance R of the mechanical arm, forward and inverse solution equations of the kinematics position, speed and acceleration of a working edge reference point are deduced. The motion control method comprises the following steps: acquiring a discrete expected position, speed and acceleration point; calculating an expected joint angle, angular velocity and angular acceleration by using a position inverse solution equation, a velocity inverse solution equation and an acceleration inverse solution equation in sequence; and giving a calculation result to a multi-axis motion controller in real time. According to the method, the radial distance of the sand disc is incorporated into the kinematics model, feedforward control of three levels of position, speed and acceleration is achieved, and the motion control precision of the non-standard mechanical arm is remarkably improved.

Description

Technical Field

[0001] This disclosure relates to the field of robot motion control technology, specifically to a method for constructing kinematic equations for a robotic arm and a motion control method based on these equations, which is particularly applicable to the forward and inverse kinematics calculation and real-time motion control of non-standard robotic arms with end effectors such as sand trays. Background Technology

[0002] With the rapid development of industrial automation, robotic arms have been widely used in industrial scenarios such as grinding, polishing, welding, and spraying. In these applications, standardized industrial robots (such as six-axis industrial robots) are usually provided with complete kinematic models and control schemes by robot manufacturers. Users can directly call the forward and inverse kinematic algorithms provided by the manufacturer to achieve precise control of the robotic arm.

[0003] However, in many practical applications, customers often design non-standard robotic arm structures due to considerations such as cost, space, and functionality. For example, in grinding applications, customers might design a non-standard three-axis serial robotic arm with a sanding disc at its end as a grinding tool. The mechanical structure of such non-standard robotic arms differs significantly from standard industrial robots; their joint configuration, arm length, and end effector mounting method are all determined by the customer based on specific requirements. Currently, there is no universal, systematic method for constructing kinematic equations and motion control schemes for non-standard robotic arms. Especially when the end effector is a tool with radial working edges, such as a sanding disc, how to incorporate the radial distance of the sanding disc into the kinematic model so that the control target is directly aligned with the actual working contact point of the sanding disc is a problem that has not been well solved in existing technologies.

[0004] Therefore, there is an urgent need for a technical solution that can systematically construct kinematic equations for non-standard robotic arms (especially robotic arms with end effectors such as sand discs) and achieve high-precision motion control based on these equations. Summary of the Invention

[0005] In view of the above-mentioned problems in the prior art, the purpose of this disclosure is to provide a method, device and system for kinematic modeling and motion control of a robotic arm, so as to solve the technical problem of non-standard robotic arms lacking kinematic forward and inverse kinematic algorithms and motion control schemes.

[0006] In a first aspect, this disclosure provides a method for constructing kinematic equations for a robotic arm, the robotic arm comprising N joints connected in series to form N arm segments, and an end effector connected to the end of the Nth arm segment, wherein a radial distance R exists between the working edge of the end effector and the end of the Nth arm segment, and N is an integer greater than 2; the method includes: determining a reference point for the working edge of the end effector, the position, velocity, and acceleration of which will serve as the target for motion control of the robotic arm; and deriving, based on the mechanical structural parameters of the robotic arm, joint variables, and the radial distance R, the forward and inverse kinematic equations for the working edge reference point of the end effector, the forward and inverse kinematic equations for the kinematic position, the forward and inverse kinematic equations for the kinematic velocity, and the forward and inverse kinematic equations for the kinematic acceleration, wherein the forward equations describe the mapping relationship from joint space variables to the position, velocity, and acceleration of the working edge reference point of the end effector in Cartesian space, and the inverse equations describe the mapping relationship from the desired position, desired velocity, and desired acceleration of the working edge reference point of the end effector in Cartesian space to the desired joint angle, desired joint angular velocity, and desired joint angular acceleration in joint space.

[0007] In a preferred embodiment, deriving the forward and inverse kinematic equations of the working edge reference point of the end effector based on the mechanical structural parameters, joint variables, and radial distance R of the robotic arm includes: obtaining the Jacobian matrix J of the working edge reference point of the end effector by differentiating the joint variables according to the forward kinematic equation of the working position, wherein the Jacobian matrix describes the mapping relationship between the linear velocity and angular velocity of the working edge reference point of the end effector and the joint angular velocity; obtaining the time derivative J̇ of the Jacobian matrix J; constructing the forward and inverse kinematic equations of the kinematic velocity based on the Jacobian matrix J; and constructing the forward and inverse kinematic equations of the kinematic acceleration based on the time derivative J̇ of the Jacobian matrix.

[0008] Secondly, this disclosure provides a motion control method for a robotic arm, the robotic arm comprising N series-connected joints forming N arm segments, and an end effector connected to the end of the Nth arm segment, wherein a radial distance R exists between the working edge of the end effector and the end of the Nth arm segment, and N is an integer greater than 2; the method includes:

[0009] (a) Obtain a series of discrete working edge reference points for the end effector, including the desired position, desired velocity, and desired acceleration.

[0010] (b) For each point on the trajectory with a desired position, desired velocity, and desired acceleration, the following steps are performed to calculate the desired joint angle, desired joint angular velocity, and desired joint angular acceleration corresponding to each point in real time: i. Based on the desired position of the working edge reference point of the end effector, the desired joint angle is calculated using the preset inverse kinematic equation of the manipulator, wherein the inverse kinematic equation is established for the working edge reference point of the end effector considering the radial distance R of the end effector; ii. Based on the desired velocity of the working edge reference point of the end effector and the calculated desired joint angle, the desired joint angle is calculated using the preset kinematic equation of the manipulator. The inverse kinematic velocity equation of the arm is used to calculate the desired joint angular velocity, wherein the inverse velocity equation is based on the Jacobian matrix J established for the working edge reference point of the end effector considering the radial distance R of the end effector; iii. Based on the desired acceleration at the working edge reference point of the end effector and the desired joint angle and the desired joint angular velocity obtained by calculation, the desired joint angular acceleration is calculated using the preset inverse kinematic acceleration equation of the manipulator, wherein the inverse acceleration equation is based on the Jacobian matrix J and its derivative J̇ established for the working edge reference point of the end effector considering the radial distance R of the end effector;

[0011] (c) The desired joint angle, desired joint angular velocity and desired joint angular acceleration corresponding to each calculated point are given to the multi-axis motion controller in real time to drive the joints of the robotic arm to move.

[0012] In a preferred embodiment, before step (a), the method further includes: generating a series of discrete points representing the desired positions, velocities, and accelerations of the end effector's working edge reference points based on the preset desired trajectory, velocity, and acceleration of the end effector's working edge reference points.

[0013] In a preferred embodiment, the method further includes: (d) during the movement of the robotic arm, acquiring the actual joint angles, joint angular velocities, and joint angular accelerations of the robotic arm in real time; (e) based on the real-time acquired actual joint angles, joint angular velocities, and joint angular accelerations of the robotic arm, using preset forward kinematic equations for the working edge reference point of the end effector considering the radial distance R of the end effector, calculating in real time the actual position, actual velocity, and actual acceleration of the working edge reference point of the end effector; (f) comparing the actual position, actual velocity, and actual acceleration of the working edge reference point of the end effector with the desired position, desired velocity, and desired acceleration, generating a joint space compensation amount based on the comparison result, and superimposing the joint space compensation amount on the desired joint angle, desired joint angular velocity, and desired joint angular acceleration in real time to correct the movement of the robotic arm.

[0014] In a preferred embodiment, the method further includes: (g) based on the calculated series of desired joint angles, desired joint angular velocities, and desired joint angular accelerations, using preset forward kinematic equations for the manipulator's kinematic position, forward kinematic velocity, and forward kinematic acceleration established for the working edge reference point of the end effector considering the radial distance R of the end effector, calculating the forward verification position, forward verification velocity, and forward verification acceleration of the working edge reference point of the end effector, and comparing them with the desired position, desired velocity, and desired acceleration of the working edge reference point of the end effector generated in step (a) to verify the correctness of the trajectory planning and inverse kinematic calculation.

[0015] In a preferred embodiment, the number of joints of the robotic arm is N=3.

[0016] In a preferred embodiment, the end effector is a sand disc.

[0017] Thirdly, this disclosure provides a kinematic equation construction device for a robotic arm, the robotic arm comprising N series-connected joints forming N arm segments, and an end effector connected to the end of the Nth arm segment, wherein a radial distance R exists between the working edge of the end effector and the end of the Nth arm segment, and N is an integer greater than 2; the device includes: a reference point determination module for determining a reference point of the working edge of the end effector, the position, velocity, and acceleration of which will serve as the target for the motion control of the robotic arm; and a kinematic equation derivation module. Based on the mechanical structural parameters, joint variables, and radial distance R of the robotic arm, this paper derives the forward and inverse kinematic equations for the working edge reference point of the end effector, including forward and inverse kinematic equations for position, velocity, and acceleration. The forward equations describe the mapping relationship from joint space variables to the position, velocity, and acceleration of the working edge reference point of the end effector in Cartesian space. The inverse equations describe the mapping relationship from the desired position, desired velocity, and desired acceleration of the working edge reference point of the end effector in Cartesian space to the desired joint angle, desired joint angular velocity, and desired joint angular acceleration in joint space.

[0018] Fourthly, this disclosure provides a motion control device for a robotic arm, characterized in that the robotic arm includes N joints connected in series to form N arm segments, and an end effector connected to the end of the Nth arm segment, wherein a radial distance R exists between the working edge of the end effector and the end of the Nth arm segment, and N is an integer greater than 2; the device includes:

[0019] The trajectory data acquisition module is used to acquire a series of discrete trajectory points of the working edge reference points of the end effector, including the desired position, desired velocity, and desired acceleration.

[0020] A real-time inverse kinematics calculation module is used to calculate the desired joint angle, desired joint angular velocity, and desired joint angular acceleration for each trajectory point on the trajectory with a desired position, desired velocity, and desired acceleration. The real-time inverse kinematics calculation module includes: a position inverse kinematics unit, used to calculate the desired joint angle based on the desired position of the end effector's working edge reference point and using a preset kinematic position inverse kinematics equation of the robotic arm, wherein the position inverse kinematics equation is established for the working edge reference point of the end effector considering the radial distance R; and a velocity inverse kinematics unit, used to calculate the desired joint angle based on the desired velocity of the end effector's working edge reference point and the desired joint angle calculated by the position inverse kinematics unit, using a preset kinematic position inverse kinematics equation. The inverse kinematic velocity equation of the robotic arm is given, and the desired joint angular velocity is calculated. The inverse velocity equation is based on the Jacobian matrix J established for the working edge reference point of the end effector considering the radial distance R. The inverse acceleration unit is used to calculate the desired joint angular acceleration based on the desired acceleration of the working edge reference point of the end effector, the desired joint angle calculated by the inverse position unit, and the desired joint angular velocity calculated by the inverse velocity unit, using the preset inverse kinematic acceleration equation of the robotic arm. The inverse acceleration equation is based on the Jacobian matrix J and its derivative J̇ established for the working edge reference point of the end effector considering the radial distance R.

[0021] The motion controller is used to receive the desired joint angle, desired joint angular velocity and desired joint angular acceleration corresponding to each trajectory point calculated by the real-time inverse kinematics calculation module, and drive the joints of the robotic arm to move.

[0022] Fifthly, this disclosure provides a motion control system for a robotic arm, comprising: a motion control device for the robotic arm as described in the second aspect above; and the multi-axis motion controller.

[0023] This disclosure, by incorporating the radial distance R of the sand disc into the kinematic model, establishes a complete system of forward and inverse kinematic equations with the working edge reference point of the sand disc as the control target, including three levels: position, velocity, and acceleration. Based on this equation system, this disclosure further provides a motion control method. By simultaneously providing the expected values ​​of the three levels of position, velocity, and acceleration to a multi-axis motion controller, feedforward control is achieved, significantly improving the dynamic tracking accuracy of non-standard robotic arms. Compared with existing technologies, the main technical advantages of this disclosure are: 1) It fills the gap in kinematic algorithms for non-standard robotic arms, providing a systematic method for constructing kinematic equations and motion control schemes for customers' self-designed non-standard robotic arms. 2) By incorporating the geometric parameters (radial distance R) of the end effector (sand disc) into the kinematic model, the target point of motion control is directly aligned with the actual working position, eliminating the control accuracy loss caused by ignoring the geometric offset of the end effector in traditional methods. 3) It not only provides forward and inverse kinematics solutions at the position level, but also systematically derives forward and inverse kinematic equations at the velocity and acceleration levels, enabling the robotic arm to achieve feedforward control at three levels: position, velocity, and acceleration, significantly improving dynamic tracking accuracy and motion smoothness. 4) It provides a real-time feedback correction mechanism and a forward and inverse kinematics verification mechanism based on the forward equations, further ensuring the accuracy and reliability of motion control. Attached Figure Description

[0024] Figure 1 A flowchart of a method for constructing kinematic equations for a robotic arm according to one embodiment of this disclosure is shown.

[0025] Figure 2 A flowchart of a motion control method for a robotic arm according to one embodiment of the present disclosure is shown.

[0026] Figure 3 A flowchart of a motion control method for a robotic arm according to another embodiment of this disclosure is shown.

[0027] Figure 4 A structural diagram of a device for constructing kinematic equations for a robotic arm according to one embodiment of the present disclosure is shown.

[0028] Figure 5 A structural diagram of a motion control device for a robotic arm according to one embodiment of the present disclosure is shown.

[0029] Figure 6 A structural diagram of a motion control system for a robotic arm according to one embodiment of the present disclosure is shown.

[0030] Figure 7 A structural diagram of an electronic device according to one embodiment of the present disclosure is shown.

[0031] List of reference numerals in the attached diagram:

[0032] Kinematic equation construction device 400;

[0033] Reference point determination module 410;

[0034] Kinematic equation derivation module 420;

[0035] Motion control device 500;

[0036] Trajectory data acquisition module 510;

[0037] Real-time inverse kinematics calculation module 520;

[0038] Motion controller 530;

[0039] Motion control system 600;

[0040] Multi-axis motion controller 610;

[0041] Electronic equipment 700;

[0042] Processor 702;

[0043] Communication interface 704;

[0044] Memory 706;

[0045] Bus 708;

[0046] Program 710. Detailed Implementation

[0047] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided with reference to the accompanying drawings and embodiments. Obviously, the described embodiments are merely some embodiments of this application, and not all embodiments. All other technical solutions obtained by those skilled in the art based on the embodiments of this application fall within the scope of protection of this application.

[0048] To facilitate understanding of the technical solutions of this disclosure, the main terms involved in this disclosure are first explained: The "robotic arm" mentioned in this disclosure refers to a mechanical structure composed of multiple joints connected in series. Each joint can rotate around its axis, thereby driving the movement of the connected arm segment. The robotic arm in this disclosure refers to a non-standard robotic arm, specifically including an actuator installed at the end of the last arm segment. The actuator has a certain radius R, and there is a radial distance R between the working edge of the actuator (i.e., the part where the sand disc contacts the workpiece) and the mounting center of the sand disc. The "joint" mentioned in this disclosure is a rotating unit in the robotic arm that can move independently. The joint is a rotary joint, and each joint rotates around its axis by an angle θ. In other embodiments, the joint can also be a sliding joint or other types of joints. The "arm segment" mentioned in this disclosure is a rigid link connecting two adjacent joints, and each arm segment has a certain length. The "working edge reference point" mentioned in this disclosure serves as the target for motion control, rather than the traditional end flange center point, thereby enabling more precise alignment of motion control with the actual working position. The "Cartesian space" mentioned in this disclosure describes the position and orientation of the actuator. The coordinates in the Cartesian space are (Y, Z, A), where Y and Z are the position coordinates and A is the orientation angle. The "joint space" mentioned in this disclosure describes the space containing the angles of the robotic arm's joints. In the embodiments of this disclosure, the variables of the joint space are (θ1, θ2, θ...). A3 The "mechanical structure parameters" mentioned in this disclosure describe the parameters of the robotic arm's geometry, including the lengths of each arm segment (L1, L2, ..., L...). N The "multi-axis motion controller" disclosed herein is a controller capable of simultaneously controlling multiple motion axes, used to receive desired joint angles, desired joint angular velocities, and desired joint angular accelerations, and to drive each joint motor to move according to the desired values.

[0049] I. Specific Implementation Examples of Non-Standard Robotic Arms

[0050] The robotic arm (i.e., N=3) of this disclosure includes three series-connected rotary joints A1, A2, and A3, forming three arm segments with lengths L1, L2, and L3, respectively. A sanding disc, with radius R, is mounted at the end of the third arm segment as an end effector. The joint angles of the three joints A1, A2, and A3 are denoted as θ1, θ2, and θ3, respectively. A3 Where θ1 is the rotation angle of the first joint A1 relative to its zero position, θ2 is the rotation angle of the second joint A2 relative to its zero position, and θ A3Let A3 be the rotation angle of the third joint relative to its zero position. The World Coordinate System (WCS) of the robotic arm is defined as (Y, Z, A), where Y is the horizontal coordinate, Z is the vertical coordinate, and A is the attitude angle of the end effector. Since the sanding disc is installed at the end of the third arm segment, and there is a radial distance R between the working edge of the sanding disc and the end of the third arm segment, the radial distance R of the sanding disc needs to be taken into account when establishing the kinematic model.

[0051] Specifically, the equivalent arm length L is defined. r and equivalent angle as follows:

[0052] ;

[0053] ;

[0054] Among them, L r θ3 is the equivalent distance from the end of the second arm segment (the beginning of the third arm segment) to the reference point of the working edge of the sand tray. θ3 is the equivalent angle of the third joint after considering the radial offset of the sand tray. arctan(R / L3) represents the angular offset of the reference point of the working edge of the sand tray relative to the axial direction of the third arm segment due to the existence of the radial distance R of the sand tray.

[0055] It should be noted that although this embodiment uses a three-axis serial robotic arm as an example, the method of this disclosure is also applicable to robotic arms with more than 3 joints N. For the case of N>3, the derivation process of the kinematic equations is similar, only the dimension and complexity of the equations will increase accordingly. Furthermore, although the end effector in this embodiment is a sanding disc, the method of this disclosure is also applicable to other types of end effectors, as long as there is a radial distance R between the working edge of the end effector and the end of the last arm segment. For example, the end effector can be a polishing disc, a circular saw, or other tools with a circular working edge.

[0056] II. Specific Implementation Methods for Constructing Kinematic Equations

[0057] The following combination Figure 1 This document details the specific implementation process of the kinematic equation construction method for robotic arms disclosed herein.

[0058] like Figure 1 As shown, in step S110, a reference point is determined on the working edge of the sand disc. This reference point is a specific point on the working edge of the sand disc, and its position, velocity, and acceleration will serve as the target for the motion control of the robotic arm.

[0059] In grinding applications, the reference point for the working edge of the sanding disc is typically selected as the point of contact between the disc and the workpiece. Since the sanding disc is circular, the position of this reference point depends on the disc's mounting method and the grinding direction. In this embodiment, the reference point is selected as a point on the circumference of the sanding disc, offset radially by a distance R from the end of the third arm segment. The significance of determining the reference point lies in the fact that traditional robotic arm motion control usually uses the center point of the end flange (i.e., the center of the output end of the last joint) as the control target. However, for a robotic arm with a sanding disc, the actual working contact point is not the center point of the end flange, but rather a point on the working edge of the sanding disc. If the center point of the end flange is still used as the control target, there will be a deviation between the actual working position and the desired position of the sanding disc, equal to the radial distance R of the disc. By using the reference point for the working edge as the control target, this deviation can be eliminated, thereby improving the accuracy of the grinding operation.

[0060] In step S120, the forward and inverse kinematic equations are derived. Specifically, based on the mechanical structural parameters (L1, L2, L3) of the robotic arm, the joint variables (θ1, θ2, θ3), and the radial distance R of the sand disc, the forward and inverse kinematic equations for the working edge reference point of the sand disc are derived. These are described below:

[0061] (1) Kinematic position forward equation

[0062] The forward kinematic equations describe the mapping from joint space variables (θ1, θ2, θ3) to the position (Y, Z, A) of the working edge reference point of the sand table in Cartesian space. For the three-axis serial robotic arm in this embodiment, the forward kinematic equations are as follows:

[0063] ①

[0064] ②

[0065] ③

[0066] As can be seen from the above equation, the radial distance R of the sand disc passes through the equivalent arm length L. r The equivalent angle θ3 was incorporated into the kinematic model. This means that the forward equations directly calculate the position of the reference point at the working edge of the sand disc, rather than the position of the center point of the end flange.

[0067] (2) Inverse kinematic position equations

[0068] The inverse kinematic position equation describes the desired position (Y) in Cartesian space from the working edge reference point of the sand table. YZA Z YZA AYZA The mapping relationship from the expected joint angles (θ1, θ2, θ3) in the joint space.

[0069] The equation for the inverse solution is:

[0070] ;

[0071] ;

[0072]

[0073] in, , , , (Y YZA Z YZA A YZA (Y,Z) represents the desired position and orientation of the reference point at the working edge of the sand table in Cartesian space, and (Y,Z) represents the position coordinates of the output end of the second joint after transformation.

[0074] By solving the inverse equations described above, given the desired position and orientation of the reference point at the working edge of the sand table in Cartesian space, the required angles for each joint can be calculated. It should be noted that in the inverse equations described above... There may be two solutions (corresponding to the "on-elbow" and "under-elbow" configurations of the robotic arm). In practical applications, it is necessary to select the appropriate solution based on the physical constraints of the robotic arm and the current configuration.

[0075] (3) Forward and inverse kinematic velocity equations

[0076] The forward and inverse kinematic velocity equations are established based on the Jacobian matrix J. The Jacobian matrix J is the partial derivative matrix of the forward kinematic position equations (i.e., formulas ①②③) with respect to the joint variables (θ1, θ2, θ3).

[0077] Taking the partial derivatives of formulas ①②③ with respect to θ1, θ2, and θ3 respectively, we can obtain the Jacobian matrix J as follows:

[0078] ④

[0079] Based on the Jacobian matrix J, the forward kinematic equations for velocity are:

[0080] ⑤

[0081] in, Let be the velocity vector of the reference point at the working edge of the sand disc in Cartesian space. These are the linear velocities in the Y and Z directions, respectively. Let A be the angular velocity. This is the joint angular velocity vector.

[0082] The meaning of the forward kinematic velocity solution is that, given the angular velocities of each joint and the current Jacobian matrix J (determined by the current joint angle), the velocity of the reference point at the working edge of the sand disc in Cartesian space can be calculated using formula ⑤.

[0083] The inverse kinematic velocity equation is:

[0084] ⑥

[0085] Among them, J -1 Let J be the inverse of the Jacobian matrix J.

[0086] The inverse kinematic velocity solution means that, given the desired velocity of the reference point at the working edge of the sand disc in Cartesian space and the inverse of the current Jacobian matrix (determined by the current joint angle), the required angular velocity of each joint can be calculated using formula ⑥.

[0087] It should be noted that the inverse velocity solution requires the Jacobian matrix J to be invertible, i.e., the determinant of J is not zero. When the determinant of J is zero, the robotic arm is in a singular configuration, and the inverse velocity solution does not exist or is not unique. In practical applications, it is necessary to avoid the robotic arm entering a singular configuration through trajectory planning (for example, when the Jacobian matrix J is close to or in a singular state, the range of arctanα in the solution of the inverse kinematic velocity equation and / or the inverse kinematic acceleration equation is judged, and α is compensated according to the judgment result so that α is within the range of arctanα), or methods such as damped least squares are used to handle the inverse solution problem near the singular configuration.

[0088] (4) Forward and inverse kinematic acceleration equations

[0089] The forward and inverse kinematic acceleration equations are established based on the Jacobian matrix J and its time derivative J̇. Taking the time derivative of the Jacobian matrix J (Equation ④) yields the time derivative J̇. Each element of J̇ is the time derivative of the corresponding element of J. Since the elements of J are functions of joint angles θ1, θ2, and θ3, and joint angles are functions of time, J̇ can be calculated using the chain rule.

[0090] Differentiating both sides of equation ⑤ with respect to time, we can obtain the forward equation for kinematic acceleration:

[0091] ⑦

[0092] in, Let be the acceleration vector of the reference point at the working edge of the sand disc in Cartesian space. These are the linear accelerations in the Y and Z directions, respectively. Let A be the angular acceleration at attitude angle A. This is the joint angular acceleration vector.

[0093] Differentiating both sides of equation ⑥ with respect to time, we can obtain the inverse equation of kinematic acceleration:

[0094] ⑧

[0095] Through the above steps, the forward and inverse kinematic equations of the working edge reference point of the sand table, the forward and inverse kinematic equations of the kinematic velocity, and the forward and inverse kinematic acceleration are fully constructed.

[0096] III. Specific Implementation Methods for Motion Control of Robotic Arms

[0097] The following combination Figure 2 and Figure 3 This section details the specific implementation process of the motion control method for robotic arms disclosed herein.

[0098] like Figure 2 As shown, in step S210, discrete points representing desired position, desired velocity, and desired acceleration are obtained. First, a series of discrete points representing the desired position, desired velocity, and desired acceleration of the sand disc working edge reference points are obtained. These discrete points constitute the desired motion trajectory of the sand disc working edge reference points in Cartesian space. In one embodiment, these discrete points can be generated through trajectory planning. Specifically, based on the preset desired trajectory (e.g., a grinding path along the workpiece surface), desired velocity (e.g., grinding speed), and desired acceleration (e.g., an acceleration / deceleration curve) of the sand disc working edge reference points, a series of discrete points representing desired position, desired velocity, and desired acceleration are generated through interpolation or sampling. For example, assuming the desired trajectory is a straight line with a starting point (Y0, Z0, A0) and an ending point (Y1, Z1, A1), and the desired velocity is a constant value V, the trajectory can be sampled at fixed time intervals Δt to generate a series of discrete points. Each discrete point contains the desired position (Yi, Zi, Ai) and desired velocity (v) at that moment. Yi , v Zi , v Ai ) and desired acceleration (a Yi , a Zi ,a Ai In another implementation, these discrete points can also be provided directly by a host computer or an external system, such as grinding path points generated by a CAD / CAM system.

[0099] In step S220, the desired joint angle, desired joint angular velocity, and desired joint angular acceleration are calculated in real time. For each point on the trajectory with a desired position, desired velocity, and desired acceleration, the corresponding desired joint angle, desired joint angular velocity, and desired joint angular acceleration are calculated sequentially according to the following sub-steps. Specifically, the implementation is as follows: In step S220a, the desired joint angle is calculated using the inverse position equation. Based on the desired position (Y) of the sand table working edge reference point at the current point... YZA Z YZA A YZA In step S220a, the desired joint angles (θ1, θ2, θ3) are calculated using the aforementioned inverse kinematic position equation, wherein the inverse position equation is established with respect to the working edge reference point of the end effector considering the radial distance R of the end effector. In step S220b, the desired joint angular velocity is calculated using the inverse velocity equation. Based on the desired velocity at the current point of the sand table working edge reference point and the desired joint angles (θ1, θ2, θ3) calculated in step S220a, the desired joint angular velocity is calculated using the aforementioned inverse kinematic velocity equation, wherein the inverse velocity equation is based on the Jacobian matrix J established with respect to the working edge reference point of the end effector considering the radial distance R of the end effector. In step S220c, the desired joint angular acceleration is calculated using the inverse acceleration equation, wherein the inverse acceleration equation is based on the Jacobian matrix J and its derivative J̇ established with respect to the working edge reference point of the end effector considering the radial distance R of the end effector. Based on the desired acceleration at the current working edge reference point of the sand table, the desired joint angles (θ1, θ2, θ3) calculated in step S220a, and the desired joint angular velocity calculated in step S220b, the desired joint angular acceleration is calculated using the preset inverse kinematic acceleration equation. It is important to note that steps S220a, S220b, and S220c are executed sequentially because the inverse velocity solution requires the results of the inverse position solution (joint angles), and the inverse acceleration solution requires the results of both the inverse position and velocity solutions (joint angles and joint angular velocities).

[0100] In step S230, the calculation results are fed into the multi-axis motion controller in real time. The desired joint angle, desired joint angular velocity, and desired joint angular acceleration corresponding to each point calculated in step S220 are fed into the multi-axis motion controller in real time to drive the movement of each joint of the robotic arm. After receiving the desired joint angle, desired joint angular velocity, and desired joint angular acceleration, the multi-axis motion controller can use the desired joint angle as the reference input for the position loop, the desired joint angular velocity as the feedforward input for the velocity loop, and the desired joint angular acceleration as the feedforward input for the acceleration loop (or current loop). By simultaneously providing the desired values ​​at the three levels of position, velocity, and acceleration, feedforward control can be achieved, significantly improving the dynamic tracking accuracy of the robotic arm and reducing tracking errors.

[0101] Compared to traditional control methods that only provide the desired joint angle, the method disclosed herein provides the desired joint angular velocity and desired joint angular acceleration, enabling the motion controller to "predict" the trend of joint movement in advance, thereby reducing the response delay of the control system and improving the accuracy and smoothness of trajectory tracking.

[0102] In another optional embodiment, the motion control method of this disclosure may further include a real-time feedback correction step based on forward equations. For example... Figure 3 As shown, it also includes steps S240 to S270.

[0103] like Figure 3 As shown, in step S240, the actual joint state is collected in real time. During the movement of the robotic arm based on steps S210, S220 and S230, the actual joint angle, joint angular velocity and joint angular acceleration of the robotic arm are collected in real time by encoders or other sensors installed on each joint.

[0104] In step S250, the actual Cartesian space state is calculated using forward kinematic equations. Specifically, based on the real-time acquired joint angles, the actual position of the reference point at the working edge of the sand table is calculated using forward kinematic equations (Formulas ①②③); based on the real-time acquired joint angular velocities, the actual velocity of the reference point at the working edge of the sand table is calculated using forward kinematic equations (Formula ⑤); and based on the real-time acquired joint angular acceleration, joint angular velocity, and joint angles, the actual acceleration of the reference point at the working edge of the sand table is calculated using forward kinematic equations.

[0105] In step S260, compensation quantities are generated and motion is corrected. The actual position, actual velocity, and actual acceleration of the reference point at the working edge of the sand table are compared with the corresponding desired position, desired velocity, and desired acceleration. Position error, velocity error, and acceleration error are calculated. Based on these errors, a joint space compensation quantity is generated using a preset control algorithm (e.g., PID control, adaptive control, etc.). This compensation quantity includes joint angle compensation, joint angular velocity compensation, and joint angular acceleration compensation. It should be understood that the data acquisition in step S240 and the data acquisition in step S260 are typically superimposed with a deviation from the scanning cycle of a multi-axis controller.

[0106] In step S270, the joint space compensation is superimposed on the desired joint angle, desired joint angular velocity and desired joint angular acceleration in real time to form the corrected joint space command, and the corrected command is sent to the multi-axis motion controller to correct the movement of the robotic arm.

[0107] Through the feedback correction mechanism described in steps S240 to S270, motion deviations caused by mechanical errors, load changes, external disturbances, and other factors can be compensated in real time, thereby further improving the motion accuracy of the robotic arm.

[0108] In an optional embodiment, the motion control method of this disclosure may further include a forward and inverse kinematics verification step. In this verification step, based on a series of desired joint angles, desired joint angular velocities, and desired joint angular accelerations obtained through inverse kinematics calculation in step S220, the forward kinematics position equation, forward kinematics velocity equation, and forward kinematics acceleration equation are used to calculate the forward verification position, forward verification velocity, and forward verification acceleration of the sand table working edge reference point. The forward verification position, forward verification velocity, and forward verification acceleration are compared with the desired position, desired velocity, and desired acceleration in step S210. If the difference between the two is within a preset error threshold range, it indicates that the inverse kinematics calculation is correct; if the difference exceeds the error threshold, it indicates that there may be an error in the inverse kinematics calculation, and the kinematic model and inverse kinematics algorithm need to be checked. The significance of this verification step is that the derivation and implementation process of inverse kinematics is relatively complex and prone to errors in formula derivation or programming implementation. Through forward kinematics verification, these errors can be detected and corrected before the actual movement of the robotic arm, avoiding abnormal robotic arm movement or even collision accidents caused by inverse kinematics errors.

[0109] Furthermore, although the above embodiments are illustrated using a three-axis serial robotic arm (N=3) as an example, the method disclosed herein is also applicable to robotic arms with a joint number N greater than 3. For the case of N>3, the forward kinematic position equation can be generalized as:

[0110] + ;

[0111] Z= ;

[0112] A= ;

[0113] Among them, L r L is the equivalent distance from the end of the (N-1)th arm segment (the beginning of the third arm segment) to the reference point at the working edge of the sand table. r = , This represents the actual angle of the m-th joint (m < N). The actual angle of the Nth joint To account for the equivalent Nth joint angle after the radial offset of the sand disc, + Correspondingly, the dimension of the Jacobian matrix J is expanded from 3×3 to M×N (where M is the number of degrees of freedom in Cartesian space and N is the number of joints), and its elements are still obtained by taking the partial derivatives of each component of the above position forward equation with respect to each joint variable. The construction methods of the velocity and acceleration forward and inverse equations are exactly the same as those of the three-axis serial robotic arm, and will not be elaborated here. It should be noted that when N>3, if the number of degrees of freedom M in Cartesian space is less than the number of joints N, the robotic arm constitutes a redundant mechanism, and the Jacobian matrix J is a non-square matrix, and there is no inverse matrix in the traditional sense. In this case, the inverse matrix in the velocity and acceleration inverse solutions needs to be replaced with a pseudo-inverse matrix (such as the Moore-Penrose pseudo-inverse), or combined with methods such as null space projection to use redundant degrees of freedom to achieve additional optimization objectives (such as joint limit avoidance, singular configuration avoidance, etc.). Furthermore, when N>3, the inverse kinematic position solution usually does not have a closed-form analytical solution and requires numerical iterative methods (such as the Newton-Raphson iteration method, gradient descent method, etc.) to solve it. Moreover, there may be multiple solutions to the inverse solution, and the optimal solution needs to be selected based on the physical constraints of the robotic arm and the current configuration.

[0114] IV. Specific Implementation Method of Motion Control Device 500 for Robotic Arm

[0115] This disclosure also provides a kinematic equation construction device 400 for a robotic arm, the device 400 being used to implement the aforementioned kinematic equation construction method. For example... Figure 4 As shown, the device 400 includes a reference point determination module 410 and a kinematic equation derivation module 420.

[0116] The reference point determination module 410 is used to determine the working edge reference point of the end effector. The position, velocity, and acceleration of this reference point will serve as the targets for the motion control of the robotic arm. Specifically, the reference point determination module 410 determines the position definition of the working edge reference point based on the type and installation method of the end effector, and determines the position based on the geometric parameters of the end effector (such as radial distance R) and the length L of the Nth arm segment. N Calculate the equivalent arm length L r The equivalent angular offset provides fundamental parameters for the subsequent derivation of the kinematic equations. Optionally, the reference point determination module 410 receives parameters such as the end effector type and radial distance R input by the user through a human-machine interface, and automatically determines the definition and equivalent parameters of the working edge reference point accordingly. In another embodiment, the reference point determination module 410 obtains the above parameters by reading a pre-stored configuration file.

[0117] The kinematic equation derivation module 420 is used to derive the forward and inverse kinematic equations of the working edge reference point of the end effector, based on the mechanical structure parameters, joint variables, and radial distance R of the robotic arm. The kinematic equation derivation module 420 receives the equivalent parameters output by the reference point determination module 410 and the mechanical structure parameters of the robotic arm, and sequentially establishes the forward and inverse position equations. It then obtains the Jacobian matrix J by taking the partial derivative of the forward position equation to establish the forward and inverse velocity equations, and further obtains J̇ by taking the time derivative of the Jacobian matrix J to establish the forward and inverse acceleration equations. The specific derivation process of each equation is consistent with step S120 in the aforementioned kinematic equation construction method, and will not be repeated here. The kinematic equation derivation module 420 stores the derived kinematic equations in a parameterized form for subsequent motion control calls. Optionally, the kinematic equation derivation module 420 is implemented as a symbolic computation engine, capable of automatically performing symbolic derivation based on the input robotic arm structural parameters to generate analytical kinematic equations. In another embodiment, the kinematic equation derivation module 420 is implemented as a pre-set equation template library for various typical robotic arm structures. Based on the robotic arm type selected by the user and the input structural parameters, the module selects the corresponding equation template from the template library and substitutes in the specific parameter values.

[0118] At the hardware implementation level, the kinematic equation construction device 400 can be implemented as a software module running on a general-purpose computer, industrial control computer, or embedded processor. The reference point determination module 410 and the kinematic equation derivation module 420 can be implemented as independent software functional modules, or they can be integrated into the same software program.

[0119] V. Specific Implementation Methods of Motion Control Devices for Robotic Arms

[0120] This disclosure also provides a motion control device 500 for a robotic arm, the device 500 being used to implement the aforementioned motion control method. For example... Figure 5 As shown, the device 500 includes a trajectory data acquisition module 510, a real-time inverse kinematics calculation module 520, and a motion controller.

[0121] The trajectory data acquisition module 510 is used to acquire a series of discrete trajectory points representing the desired positions, velocities, and accelerations of the working edge reference points of the end effector. In one embodiment, the trajectory data acquisition module 510 has a built-in trajectory planning function, which generates a series of discrete trajectory points through interpolation and sampling based on the user-preset desired trajectory, desired motion velocity, and desired acceleration / deceleration curve. In another embodiment, the trajectory data acquisition module 510 receives pre-planned trajectory data from an external host computer or CAD / CAM system via a communication interface 704 (such as Ethernet, fieldbus 708, etc.). In yet another embodiment, the trajectory data acquisition module 510 reads a pre-stored trajectory data file from local memory.

[0122] The real-time inverse kinematics calculation module 520 is used to calculate the desired joint angle, desired joint angular velocity, and desired joint angular acceleration for each trajectory point on the trajectory in real time. The real-time inverse kinematics calculation module 520 includes a position inverse kinematics unit, a velocity inverse kinematics unit, and an acceleration inverse kinematics unit, which execute calculations sequentially. Specifically, the position inverse kinematics unit receives the desired position of the current trajectory point, calculates the desired joint angle using a preset kinematic position inverse kinematics equation, and outputs the calculation result to the velocity inverse kinematics unit and the acceleration inverse kinematics unit. The velocity inverse kinematics unit receives the desired velocity of the current trajectory point and the desired joint angle output by the position inverse kinematics unit, calculates the Jacobian matrix J and its inverse matrix under the current configuration based on the desired joint angle, calculates the desired joint angular velocity using the velocity inverse kinematics equation, and outputs the calculation result to the acceleration inverse kinematics unit and the motion controller. The inverse acceleration unit receives the desired acceleration and velocity of the current trajectory point, the desired joint angle output by the inverse position unit, and the desired joint angular velocity output by the inverse velocity unit. It calculates the time derivative J̇ of the Jacobian matrix based on the desired joint angle and desired joint angular velocity, calculates the desired joint angular acceleration using the inverse acceleration equation, and outputs the calculation result to the motion controller.

[0123] The specific forms of the inverse position equation, inverse velocity equation, and inverse acceleration equation called by each unit are consistent with those described in the aforementioned motion control method, and will not be repeated here.

[0124] It should be noted that the calculations for each trajectory point by the three inverse kinematics units need to be completed within one control cycle to meet real-time requirements. At the hardware implementation level, the real-time inverse kinematics calculation module 520 can be implemented as a real-time computing program running on a high-performance embedded processor 702 (such as a DSP, FPGA, or ARM processor). The three inverse kinematics units can be implemented as independent computing tasks executed sequentially according to priority, or as a pipelined computing architecture to improve computational efficiency. In another implementation, the real-time inverse kinematics calculation module 520 can also be implemented as a software module within a real-time operating system running on an industrial control computer.

[0125] The motion controller is a multi-axis motion controller 610, capable of simultaneously controlling N joints of the robotic arm. After receiving the desired joint angle, desired joint angular velocity, and desired joint angular acceleration output by the real-time inverse kinematics module 520, the motion controller uses the desired joint angle as the reference input for each joint position loop, the desired joint angular velocity as the feedforward input for each joint velocity loop, and the desired joint angular acceleration as the feedforward input for each joint acceleration loop (or current loop / torque loop). Through its internal servo control algorithm, it generates control signals to drive the motors of each joint, driving each joint to move according to the desired values.

[0126] In one embodiment, the motion controller is an independent multi-axis servo drive connected to the real-time inverse kinematics (IRK) module 520 via a high-speed communication bus (such as EtherCAT, PROFINET, SERCOS, etc.). In another embodiment, the motion controller and the IRD module 520 are integrated on the same hardware platform, such as on the same motion control card or in the same PLC system.

[0127] In a preferred embodiment, the motion control device 500 further includes a feedback correction module, which comprises a state acquisition unit, a forward kinematics calculation unit, and a compensation quantity generation unit. The state acquisition unit is used to acquire the actual joint angles, joint angular velocities, and joint angular accelerations of the robotic arm in real time. The forward kinematics calculation unit is used to calculate the actual position, actual velocity, and actual acceleration of the end effector's working edge reference point using the forward kinematics equations. The compensation quantity generation unit is used to compare the actual values ​​with the expected values, generate a joint space compensation quantity, and superimpose it onto the expected joint value to correct the motion. The specific process of feedback correction is consistent with steps S240 to S270 of the aforementioned motion control method, and will not be repeated here.

[0128] In another preferred embodiment, the motion control device 500 further includes a forward and inverse kinematics verification module, used to verify the inverse kinematics calculation results using the forward kinematics equations. When the verification result exceeds a preset error threshold, the forward and inverse kinematics verification module outputs an alarm signal, prompting the user to check the kinematics model and inverse kinematics algorithm. The specific process of forward and inverse kinematics verification is consistent with the "forward and inverse kinematics verification steps" described in the aforementioned motion control method, and will not be repeated here.

[0129] VI. Specific Implementation Methods of Motion Control Systems for Robotic Arms

[0130] This disclosure also provides a motion control system 600 for a robotic arm, such as... Figure 6 As shown, system 600 includes the aforementioned motion control device 500 for the robotic arm and a multi-axis motion controller 610. The workflow of system 600 is as follows: the trajectory data acquisition module 510 in the motion control device 500 acquires trajectory point data, and the real-time inverse kinematics calculation module 520 sequentially performs inverse kinematics calculations on each trajectory point to obtain the desired joint angle, desired joint angular velocity, and desired joint angular acceleration. The calculation results are then transmitted to the multi-axis motion controller 610 in real time through a communication interface. The multi-axis motion controller 610 drives the movement of each joint of the robotic arm accordingly, causing the working edge reference point of the end effector to move along the desired trajectory.

[0131] In one embodiment, the trajectory data acquisition module 510 and the real-time inverse kinematics calculation module 520 in the motion control device 500 run on a host industrial computer. The multi-axis motion controller 610 is an independent servo drive system. The host industrial computer and the multi-axis motion controller 610 are connected via a high-speed real-time communication bus (such as an EtherCAT bus). The host industrial computer completes the inverse kinematics calculation of one trajectory point in each communication cycle and sends the calculation result to the multi-axis motion controller 610 via the communication bus 708.

[0132] In another embodiment, the trajectory data acquisition module 510 and the real-time inverse kinematics calculation module 520 in the motion control device 500 run on a PLC or motion control card, and the multi-axis motion controller 610 is integrated in the same PLC system or motion control card to reduce communication delay and improve system real-time performance.

[0133] In another implementation, the trajectory data acquisition module 510 runs on a host computer, and the real-time inverse kinematics calculation module 520 and the multi-axis motion controller 610 are integrated on the same embedded motion control platform. The host computer is responsible for trajectory planning and data generation, while the embedded motion control platform is responsible for real-time inverse kinematics calculation and motion control.

[0134] When the system includes a feedback correction function, the system also includes encoders or other position / speed sensors installed on each joint of the robotic arm to collect the actual state of each joint in real time and feed it back to the feedback correction module in the motion control device 500 to form a closed-loop control.

[0135] VII. Specific Implementation Methods of Electronic Devices, Storage Media, and Computer Program Products

[0136] Figure 7 This is a schematic diagram of an electronic device according to an embodiment of this application. The specific embodiments of this application do not limit the specific implementation of the electronic device. See also... Figure 7 The electronic device 700 provided in this application embodiment includes: a processor 702, a communications interface 704, a memory 706, and a bus 708. Wherein:

[0137] The processor 702, communication interface 704, and memory 706 communicate with each other via bus 708.

[0138] Communication interface 704 is used to communicate with other electronic devices or servers.

[0139] The processor 702 is used to execute program 710, specifically the relevant steps in the above method embodiments.

[0140] Specifically, program 710 may include program code that includes computer operation instructions.

[0141] The processor 702 may be a central processing unit (CPU), an application-specific integrated circuit (ASIC), or one or more integrated circuits configured to implement the embodiments of this application. The smart device includes one or more processors, which may be processors of the same type, such as one or more CPUs; or they may be processors of different types, such as one or more CPUs and one or more ASICs.

[0142] Memory 706 is used to store program 710. Memory 706 may include high-speed RAM memory, and may also include non-volatile memory, such as at least one disk storage device.

[0143] Specifically, program 710 can be used to cause processor 702 to execute the methods in any of the foregoing embodiments.

[0144] The specific implementation of each step in program 710 can be found in the corresponding descriptions of the steps and units in the above method embodiments, and will not be repeated here. Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working process of the devices and modules described above can be referred to the corresponding process descriptions in the foregoing method embodiments, and will not be repeated here.

[0145] This application also provides a computer-readable storage medium storing instructions for causing a machine to perform the methods described herein. Specifically, a system or apparatus equipped with a storage medium storing software program code that implements the functions of any of the embodiments described above, and enabling the computer (or CPU or MPU) of the system or apparatus to read and execute the program code stored in the storage medium.

[0146] In this case, the program code read from the storage medium can itself implement the function of any of the above embodiments, and therefore the program code and the storage medium storing the program code constitute a part of this application.

[0147] Storage media embodiments for providing program code include floppy disks, hard disks, magneto-optical disks, optical disks (such as CD-ROM, CD-R, CD-RW, DVD-ROM, DVD-RAM, DVD-RW, DVD+RW), magnetic tapes, non-volatile memory cards, and ROMs. Alternatively, program code can be downloaded from a server computer via a communication network.

[0148] This application also provides a computer program product, including computer instructions that instruct a computing device to perform any corresponding operation in the above-described plurality of method embodiments.

[0149] It should be noted that, depending on the implementation needs, the various components / steps described in the embodiments of this application can be broken down into more components / steps, or two or more components / steps or parts of the operation of components / steps can be combined into new components / steps to achieve the purpose of the embodiments of this application.

[0150] The methods described in the embodiments of this application can be implemented in hardware, firmware, or as software or computer code that can be stored in a recording medium (such as a CD-ROM, RAM, floppy disk, hard disk, or magneto-optical disk), or as computer code downloaded over a network that is originally stored in a remote recording medium or a non-transitory machine-readable medium and will be stored in a local recording medium. Thus, the methods described herein can be processed by software stored on a recording medium using a general-purpose computer, a dedicated processor, or programmable or dedicated hardware (such as an ASIC or FPGA). It is understood that the computer, processor, microprocessor controller, or programmable hardware includes storage components (e.g., RAM, ROM, flash memory, etc.) capable of storing or receiving software or computer code, which, when accessed and executed by the computer, processor, or hardware, implements the methods described herein. Furthermore, when a general-purpose computer accesses code used to implement the methods shown herein, the execution of the code transforms the general-purpose computer into a dedicated computer for executing the methods shown herein.

[0151] It should be noted that not all steps and modules in the above processes and system structure diagrams are mandatory; some steps or modules can be omitted as needed. The execution order of each step is not fixed and can be adjusted as required. The system structure described in the above embodiments can be a physical structure or a logical structure. That is, some modules may be implemented by the same physical entity, or some modules may be implemented by multiple physical entities, or they may be jointly implemented by certain components in multiple independent devices.

[0152] In this patent application, nouns and pronouns relating to persons are not limited to specific genders. The terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one" does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0153] In the above embodiments, the hardware modules can be implemented mechanically or electrically. For example, a hardware module may include permanent, dedicated circuitry or logic (such as a dedicated processor, FPGA, or ASIC) to perform the corresponding operations. The hardware module may also include programmable logic or circuitry (such as a general-purpose processor or other programmable processor), which can be temporarily configured by software to perform the corresponding operations. The specific implementation method (mechanical, dedicated, permanent circuitry, or temporarily configured circuitry) can be determined based on cost and time considerations.

[0154] The present application has been shown and described in detail above with reference to the accompanying drawings and preferred embodiments. However, the present application is not limited to these disclosed embodiments. Based on the above multiple embodiments, those skilled in the art will know that more embodiments of the present application can be obtained by combining the code review methods in the different embodiments above. These embodiments are also within the protection scope of the present application.

Claims

1. A method for constructing the kinematic equations of a robotic arm, the robotic arm comprising N series-connected joints forming N arm segments, and an end effector connected to the end of the Nth arm segment, wherein a radial distance R exists between the working edge of the end effector and the end of the Nth arm segment, and N is an integer greater than 2; characterized in that, The method includes: The working edge reference point of the end effector is determined, and the position, velocity, and acceleration of the reference point will be used as the target for the motion control of the robotic arm; Based on the mechanical structure parameters, joint variables, and radial distance R of the robotic arm, the forward and inverse kinematic equations for the working edge reference point of the end effector are derived. The forward equations describe the mapping relationship from the joint space variables to the position, velocity, and acceleration of the working edge reference point of the end effector in Cartesian space. The inverse equations describe the mapping relationship from the desired position, desired velocity, and desired acceleration of the working edge reference point of the end effector in Cartesian space to the desired joint angle, desired joint angular velocity, and desired joint angular acceleration in joint space.

2. The method according to claim 1, characterized in that, Based on the mechanical structure parameters, joint variables, and radial distance R of the robotic arm, the forward and inverse kinematic equations for the working edge reference point of the end effector are derived, including: Based on the kinematic position forward equation, the Jacobian matrix J of the working edge reference point of the end effector is obtained by differentiating the joint variables. The Jacobian matrix describes the mapping relationship between the linear velocity and angular velocity of the working edge reference point of the end effector and the joint angular velocity. Taking the time derivative of the Jacobian matrix J, we obtain the time derivative J̇ of the Jacobian matrix; Based on the Jacobian matrix J, construct the forward and inverse kinematic velocity equations; Based on the time derivative J̇ of the Jacobian matrix, the forward and inverse kinematic acceleration equations are constructed.

3. A motion control method for a robotic arm, the robotic arm comprising N series-connected joints forming N arm segments, and an end effector connected to the end of the Nth arm segment, wherein a radial distance R exists between the working edge of the end effector and the end of the Nth arm segment, and N is an integer greater than 2; characterized in that, The method includes: (a) Obtain a series of discrete working edge reference points for the end effector, including the desired position, desired velocity, and desired acceleration. (b) For each point on the trajectory with a desired position, desired velocity, and desired acceleration, perform the following steps to calculate the desired joint angle, desired joint angular velocity, and desired joint angular acceleration for each point in real time: i. Based on the desired position of the working edge reference point of the end effector, the desired joint angle is calculated using the preset inverse kinematic position equation of the robotic arm, wherein the inverse position equation is established for the working edge reference point of the end effector considering the radial distance R of the end effector; ii. Based on the desired velocity at the working edge reference point of the end effector and the desired joint angle obtained by calculation, the desired joint angular velocity is calculated using the preset inverse kinematic velocity equation of the robotic arm, wherein the inverse velocity equation is based on the Jacobian matrix J established for the working edge reference point of the end effector considering the radial distance R of the end effector; iii. Based on the desired acceleration at the working edge reference point of the end effector and the desired joint angle and desired joint angular velocity obtained by calculation, the desired joint angular acceleration is calculated using the preset inverse kinematic acceleration equation of the robotic arm, wherein the inverse acceleration equation is based on the Jacobian matrix J and its derivative J̇ established for the working edge reference point of the end effector considering the radial distance R of the end effector; (c) The desired joint angle, desired joint angular velocity and desired joint angular acceleration corresponding to each calculated point are given to the multi-axis motion controller in real time to drive the joints of the robotic arm to move.

4. The method according to claim 3, characterized in that, Step (a) includes the following: Based on the preset expected trajectory, expected velocity, and expected acceleration of the end effector's working edge reference point, a series of discrete points with expected positions, expected velocities, and expected accelerations of the end effector's working edge reference point are generated.

5. The method according to claim 3, characterized in that, The method further includes: (d) During the movement of the robotic arm, the actual joint angles, joint angular velocities, and joint angular accelerations of the robotic arm are collected in real time; (e) Based on the actual joint angles, joint angular velocities, and joint angular accelerations of the robotic arm collected in real time, the actual position, actual velocity, and actual acceleration of the working edge reference point of the end effector are calculated in real time using the kinematic position forward equation, kinematic velocity forward equation, and kinematic acceleration forward equation of the robotic arm established by the working edge reference point of the end effector considering the radial distance R of the end effector. (f) The actual position, actual velocity and actual acceleration of the working edge reference point of the end effector are compared with the expected position, expected velocity and expected acceleration. Based on the comparison result, a joint space compensation amount is generated, and the joint space compensation amount is superimposed on the expected joint angle, expected joint angular velocity and expected joint angular acceleration in real time to correct the movement of the robotic arm.

6. The method according to claim 3, characterized in that, The method further includes: (g) Based on the series of expected joint angles, expected joint angular velocities, and expected joint angular accelerations obtained from the calculation, the forward kinematic position equation, forward kinematic velocity equation, and forward kinematic acceleration equation of the robotic arm are established using the preset forward kinematic position equation, forward kinematic velocity equation, and forward kinematic acceleration equation established for the working edge reference point of the end effector considering the radial distance R of the end effector. The forward verification position, forward verification velocity, and forward verification acceleration of the working edge reference point of the end effector are calculated and compared with the expected position, expected velocity, and expected acceleration of the working edge reference point of the end effector generated in step (a) to verify the correctness of the trajectory planning and inverse kinematic calculation.

7. The method according to claim 4, characterized in that, The number of joints in the robotic arm is N=3.

8. The method according to claim 4, characterized in that, The end effector is a sand disc.

9. A kinematic equation construction device (400) for a robotic arm, the robotic arm comprising N series-connected joints forming N arm segments, and an end effector connected to the end of the Nth arm segment, wherein a radial distance R exists between the working edge of the end effector and the end of the Nth arm segment, and N is an integer greater than 2; characterized in that, The device (400) includes: The reference point determination module (410) is used to determine the working edge reference point of the end effector, the position, velocity and acceleration of which will be used as the target of the motion control of the robotic arm; The kinematic equation derivation module (420) is used to derive the forward and inverse kinematic position equations, forward and inverse kinematic velocity equations, and forward and inverse kinematic acceleration equations of the working edge reference point of the end effector based on the mechanical structure parameters, joint variables, and radial distance R of the robotic arm. The forward equations describe the mapping relationship from the joint space variables to the position, velocity, and acceleration of the working edge reference point of the end effector in Cartesian space. The inverse equations describe the mapping relationship from the desired position, desired velocity, and desired acceleration of the working edge reference point of the end effector in Cartesian space to the desired joint angle, desired joint angular velocity, and desired joint angular acceleration in joint space.

10. A motion control device (500) for a robotic arm, characterized in that, The robotic arm comprises N joints connected in series to form N arm segments, and an end effector connected to the end of the Nth arm segment. A radial distance R exists between the working edge of the end effector and the end of the Nth arm segment, where N is an integer greater than 2. The device (500) is characterized by comprising: The trajectory data acquisition module (510) is used to acquire a series of discrete trajectory points of the working edge reference points of the end effector, including the desired position, desired velocity, and desired acceleration. A real-time inverse kinematics calculation module (520) is used to calculate, in real time, the desired joint angle, desired joint angular velocity, and desired joint angular acceleration corresponding to each trajectory point on the trajectory for each desired position, desired velocity, and desired acceleration. The real-time inverse kinematics calculation module (520) includes: The position inverse unit is used to calculate the desired joint angle based on the desired position of the working edge reference point of the end effector and using the preset kinematic position inverse equation of the robotic arm, wherein the position inverse equation is established for the working edge reference point of the end effector considering the radial distance R. The velocity inverse kinematics unit is used to calculate the desired joint angular velocity based on the desired velocity of the working edge reference point of the end effector and the desired joint angle calculated by the position inverse kinematics unit, using the preset kinematic velocity inverse kinematics equation of the manipulator, wherein the velocity inverse kinematics equation is based on the Jacobian matrix J established for the working edge reference point of the end effector considering the radial distance R; An inverse acceleration unit is used to calculate the desired joint angular acceleration based on the desired acceleration at the working edge reference point of the end effector, the desired joint angle calculated by the position inverse unit, and the desired joint angular velocity calculated by the velocity inverse unit, using a preset inverse kinematic acceleration equation of the robotic arm. The inverse acceleration equation is based on the Jacobian matrix J and its derivative J̇ established for the working edge reference point of the end effector considering the radial distance R. The motion controller (530) is used to receive the desired joint angle, desired joint angular velocity and desired joint angular acceleration corresponding to each trajectory point calculated by the real-time inverse kinematics calculation module, and drive each joint of the robotic arm to move.

11. A motion control system (600) for a robotic arm, characterized in that, The system (600) includes: The motion control device (500) for the robotic arm according to claim 10; and, The multi-axis motion controller (610).

Images