A robot arm pasting operation control method and system

Abstract

The application provides a mechanical arm pasting operation control method and system, constructs a complete closed-loop control framework from three-dimensional perception to accurate execution of the mechanical arm, the core advantage of which is that the traditional experience calibration is replaced by a geometric analysis method, accurate conversion from push rod displacement to joint angle is realized, and a rich inverse solution candidate set is generated through dense sampling and constraint verification, which significantly improves the flexibility and success rate of motion planning. In the obstacle avoidance and pasting link, BiRRT multi-objective planning is adopted to ensure collision-free motion in a complex environment and realize adaptive pasting without a force sensor. The overall scheme has high precision, high safety, strong adaptability and good portability, and is especially suitable for surface pasting operation scenes of outdoor heavy-load panels.

Description

Technical Field

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

[0002] Industrial robotic arms typically use rotary motors to directly drive joints, and the joint angles can be directly obtained from the motor encoder. However, in outdoor heavy-duty panel surface operations, linear actuators are widely used due to their advantages such as high thrust, dust and water resistance, and self-locking. However, there is a complex nonlinear geometric mapping relationship between the linear displacement of the actuator and the joint rotation angle.

[0003] Traditional methods for solving the inverse kinematics of 3R planar robotic arms are based on analytical geometry and can usually only provide a finite set of solutions (1 to 2 sets), lacking a systematic approach to the physical constraints of joint angles.

[0004] When the robotic arm moves in the panel array, it may collide with the panel support, adjacent components, cable trays, etc. Existing simple limiting methods cannot guarantee the safety of movement in complex environments.

[0005] In panel component surface bonding operations, the end-mounted bonding head needs to be precisely bonded to the panel surface (too large a gap will affect the work effect, and excessive pressure will damage the component surface), and existing methods lack a precise bonding mechanism. Summary of the Invention

[0006] This invention addresses the technical problems existing in the prior art by providing a robotic arm fitting operation control method and system based on push rod encoder calculation and obstacle avoidance planning, which solves the control accuracy problem of push rod driven robotic arms in complex panel operation scenarios.

[0007] According to a first aspect of the present invention, a robotic arm fitting operation control method based on pushrod encoder calculation and obstacle avoidance planning is provided, comprising: The three-dimensional target pose of the target panel component surface is obtained, and the three-dimensional target pose is projected onto the working plane of the robotic arm to generate the target anchor point position and target fitting angle in the two-dimensional plane. The encoder values ​​of the push rod motors driving each joint of the robotic arm are read in real time and converted into the actual linear displacement of each push rod. Based on the pre-stored geometric parameters of the robotic arm links, the actual linear displacement of each push rod is converted into the rotation angle of the corresponding joint through the chain derivation of the cosine theorem of multi-level triangles, thus obtaining the current joint configuration. Sampling is performed within the contact angle constraint range of the end effector. Based on the target anchor point position and target contact angle, combined with vertical constraint conditions and joint limit constraints, a set of feasible inverse kinematic target configurations that satisfy all constraints is generated. Based on the current joint configuration and the set of feasible inverse kinematics target configurations, a collision-free motion trajectory from the current configuration to the target configuration is planned under the constraints of the collision model. The robotic arm joints are driven to move along the collision-free motion trajectory to the target configuration, and the end-effector is controlled to advance towards the surface of the target panel component until a safe fitting position is reached.

[0008] Based on the above technical solution, the present invention can also be improved as follows.

[0009] Optionally, the three-dimensional target pose includes anchor point position and normal vector direction. Projecting the three-dimensional target pose onto the working plane of the robotic arm generates the target anchor point position and target fitting angle in the two-dimensional plane, including: Define a coordinate system for the turntable working plane with the turntable rotation center as the origin, the turntable orientation direction as the X-axis, and the vertical upward direction as the Z-axis; The reference points at the anchor point position and in the direction of the normal vector are projected onto the coordinate system of the turntable working plane to obtain the two-dimensional anchor point coordinates and the two-dimensional normal point coordinates. The target fitting angle is calculated based on the two-dimensional anchor point coordinates and the two-dimensional normal point coordinates.

[0010] Optionally, the robotic arm is a 3-DOF robotic arm comprising an upper arm joint, a forearm joint, and a wrist joint, each joint being independently driven by a first push rod, a second push rod, and a third push rod, respectively; based on pre-stored robotic arm link geometry parameters, through a chain derivation using the cosine theorem of multi-level triangles, the actual linear displacement of each push rod is converted into the rotation angle of the corresponding joint to obtain the current joint configuration, including: During system initialization, multiple fixed angles are pre-calculated based on the length of fixed members that does not change with the push rod. These fixed angles are obtained by using the law of cosines from the lengths of the three members that form the corresponding fixed triangle. For the upper arm joint, based on the linear displacement of the first push rod, the intermediate angle is solved by the one-step triangle cosine theorem, and combined with the pre-calculated fixed angle, the rotation angle of the upper arm joint is calculated. For the forearm joint, based on the linear displacement of the second push rod, the rotation angle of the forearm joint is obtained by solving the triangle cosine theorem in six steps and calculating the auxiliary side in one step. For the wrist joint, based on the linear displacement of the third push rod, the wrist joint rotation angle is obtained by solving the triangle cosine theorem in four steps and calculating the auxiliary side in one step. The current joint configuration is obtained based on the rotation angles of the upper arm joint, forearm joint, and wrist joint. The auxiliary side is obtained by using the law of cosines to find the opposite side from two known sides and their included angle in a pre-established triangle model; the intermediate angle is obtained by using the law of cosines to find the angle from three known sides in a pre-established triangle model.

[0011] Optionally, sampling is performed within the contact angle constraint range of the end effector. Based on the target anchor point position and target contact angle, combined with vertical constraints and joint limit constraints, a set of feasible inverse kinematic target configurations satisfying all constraints is generated, including: N sampling angle values ​​are uniformly sampled within a preset end-fitting angle range; For each sampled angle value, the direction of the end link is determined based on the target anchor point position, and the length of the end link is offset in the opposite direction from the target anchor point position along the end link direction to obtain the target position of the wrist joint; Based on the vertical coordinate constraint of the target position of the wrist joint, the combination of joint angles of the upper arm joint and the forearm joint is solved. When the target position of the wrist joint exceeds the maximum extension range of the upper arm joint and the forearm joint, it is determined that there is no solution and the current sampled angle value is skipped; otherwise, the current sampled angle value is retained. For each set of joint angles obtained by the solution, verify whether they meet the angle limit constraints of the corresponding joint. All joint angles that have passed the joint limit verification are arranged according to a preset sorting rule to form the set of feasible inverse kinematic target configurations.

[0012] Optionally, the step of planning a collision-free motion trajectory under collision model constraints based on the current joint configuration and the set of feasible inverse kinematics target configurations includes: The loading robot arm uses the Unified Robot Description Format (URDF) kinematic chain model and the Semantic Robot Description Format (SRDF) planning group configuration. The URDF kinematic chain model defines the three-dimensional collision mesh attached to each joint type and link, and the SRDF planning group configuration specifies the subset of joints participating in the planning and self-collision exclusion pairs. Based on the anchor point position and normal vector direction of the target panel component, the target panel component is modeled as a cuboid collision obstacle and added to the three-dimensional collision model; The Bi-directional fast expanding random tree (BiRRT) algorithm is used to search in the joint space of the joint subset participating in the planning, using the set of feasible inverse kinematic target configurations as multiple candidate targets, and simultaneously expanding tree nodes to each candidate target, selecting the first optimal path reached as the collision-free motion trajectory. Specifically, each time a tree node expands, the collision detection engine is invoked to determine whether the current joint configuration collides with an obstacle in the scene based on the three-dimensional collision mesh.

[0013] Optionally, the planned collision-free motion trajectory includes: In the first stage, based on the three-dimensional collision model, the BiRRT algorithm is used to plan the collision-free trajectories of the joints, except for the end-effector suspension axis, from the current configuration to the target configuration. The end-effector suspension axis remains in its initial position, and the joints of the robotic arm are controlled to execute the corresponding collision-free motion trajectory. In Phase Two, after each joint reaches the target configuration, the end effector of the robotic arm extends gradually along its axis, and a collision detection is performed after each extension. If there is no collision, the extension continues; if a collision is detected, the extension is stopped and the arm retracts to the previous safe position, so as to achieve adaptive fitting between the end effector and the target panel component.

[0014] Optionally, the movement of each joint of the driven robotic arm along the collision-free motion trajectory to the target configuration includes: During the execution of the collision-free motion trajectory by each joint, an independent PID controller is used for closed-loop control of each joint, and the speed of each joint is dynamically adjusted according to the end-effector attitude deviation to achieve coordinated movement of multiple joints.

[0015] Optionally, the method of using an independent PID controller for closed-loop control of each joint and dynamically adjusting the speed of each joint based on the end-effector attitude deviation to achieve coordinated movement of multiple joints includes: Each joint is set with an independent PID controller to calculate the deviation between the target angle and the current angle of the end effector, and generate speed commands for each joint based on the deviation. A global velocity attenuation factor is calculated based on the Gaussian function and the deviation. When the end attitude angle or orientation angle deviation is large, the speed of the joints that are close to being in position is reduced based on the global speed attenuation factor, so that all joints can be coordinated to be in position.

[0016] Optionally, when the deviation between the actual angle and the target angle of all joints is less than their respective preset positioning thresholds, the robotic arm is determined to be in position, and the end effector is triggered to start performing panel surface processing operations.

[0017] According to a second aspect of the present invention, a robotic arm fitting operation control system based on pushrod encoder calculation and obstacle avoidance planning is provided, comprising: The acquisition module is used to acquire the three-dimensional target posture of the target panel component surface, project the three-dimensional target posture onto the working plane of the robotic arm, and generate the target anchor point position and target fitting angle in the two-dimensional plane. The conversion module is used to read the encoder values ​​of the push rod motors driving each joint of the robotic arm in real time and convert them into the actual linear displacement of each push rod. Based on the pre-stored geometric parameters of the robotic arm links, the actual linear displacement of each push rod is converted into the rotation angle of the corresponding joint through the chain derivation of the cosine theorem of multi-level triangles to obtain the current joint configuration. The generation module is used to sample within the contact angle constraint range of the end effector. Based on the target anchor point position and target contact angle, combined with vertical constraints and joint limit constraints, it generates a set of feasible inverse kinematic target configurations that satisfy all constraints. The planning module is used to plan a collision-free motion trajectory from the current configuration to the target configuration under the constraints of the collision model, based on the current joint configuration and the set of feasible inverse kinematics target configurations. The execution module is used to drive each joint of the robotic arm to move along the collision-free motion trajectory to the target configuration, and control the end-effector to advance towards the surface of the target panel component until a safe fitting position is reached.

[0018] This invention provides a robotic arm bonding operation control method and system based on pushrod encoder calculation and obstacle avoidance planning. It constructs a complete closed-loop control framework from 3D perception to precise robotic arm execution. Its core advantage lies in replacing traditional empirical calibration with a geometric analytical method, achieving precise conversion from pushrod displacement to joint angles. Furthermore, it generates a rich inverse kinematics candidate set through dense sampling and constraint verification, significantly improving the flexibility and success rate of motion planning. In the obstacle avoidance and bonding stages, BiRRT multi-objective planning is employed to ensure collision-free motion in complex environments, achieving adaptive bonding without force sensors. The overall solution combines high precision, high safety, strong adaptability, and good portability, making it particularly suitable for outdoor heavy-duty panel surface bonding operations. Attached Figure Description

[0019] Figure 1 A flowchart of a robotic arm fitting operation control method based on push rod encoder calculation and obstacle avoidance planning is provided as an embodiment of the present invention; Figure 2 This is a schematic diagram of a robot model; Figure 3 This is a structural block diagram of a robotic arm fitting operation control system based on push rod encoder calculation and obstacle avoidance planning, provided as an embodiment of the present invention. Detailed Implementation

[0020] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention. In addition, the technical features of the various embodiments or individual embodiments provided by the present invention can be arbitrarily combined with each other to form feasible technical solutions. Such combinations are not constrained by the order of steps and / or structural composition patterns, but must be based on the ability of those skilled in the art to implement them. When the combination of technical solutions is contradictory or cannot be implemented, it should be considered that such a combination of technical solutions does not exist and is not within the scope of protection claimed by the present invention.

[0021] Figure 1 A flowchart illustrating a robotic arm fitting operation control method based on pushrod encoder calculation and obstacle avoidance planning according to an embodiment of the present invention is shown. Figure 1 As shown, the method includes the following steps: Step 1: Obtain the three-dimensional target pose of the target panel component surface, project the three-dimensional target pose onto the working plane of the robotic arm, and generate the target anchor point position and target fitting angle in the two-dimensional plane.

[0022] Understandably, the three-dimensional spatial attitude of the target panel component provided by upstream sensing (such as LiDAR, vision system, etc.) is projected onto the working plane of the robotic arm turntable (the vertical plane directly in front of the turntable).

[0023] First, the working plane of the turntable is defined. Specifically, the rotation center of the turntable is taken as the origin, and the turntable's orientation angle is defined as follows: Define the work plane coordinate system:

[0024] The working plane of the robotic arm is The plane spanned (the vertical plane directly in front of the turntable, i.e., the vertical plane as the y-axis).

[0025] After defining the turntable's working plane, the 3D spatial pose of the target panel component is projected onto the robot arm's working plane. Specifically, given the 3D target points of the target panel component... (Anchor point) and (Normal direction point), projected onto the working plane of the robotic arm:

[0026]

[0027] in,( , ) refers to the x and y coordinates of the anchor point projection point. , () refers to the x and y coordinates of the projection point of the normal direction point. This refers to the coordinates of the turntable center.

[0028] After projecting the anchor point and the normal direction point respectively, calculate the end-fitting direction: .

[0029] Subsequently, the projected coordinates are transformed from the turntable plane coordinate system to the robot arm kinematic base coordinate system, taking into account the height offset between the base and the turntable. ,in, The height of the base This refers to the height of the turntable itself.

[0030]

[0031] Calculate the target fitting angle:

[0032] Output the target parameters for inverse solution , ( () indicates the target location. Indicates the angle at which the target fits.

[0033] Step 2: Read the encoder values ​​of the push rod motors driving each joint of the robotic arm in real time and convert them into the actual linear displacement of each push rod.

[0034] See also Figure 2 , Figure 2 The geometry of the mechanism is shown, with the robotic arm's three rotary joints each driven by a separate actuator motor. The linear displacement of the actuators and the joint rotation angles are transmitted via a multi-stage triangular / four-bar linkage. Key hinge points are marked in the mechanism. The lengths of each member are known mechanical design constants. The variable extensions of the three push rods are as follows: The length of the boom push rod (corresponding to joint 1) is measured by an encoder; The length of the forearm push rod (corresponding to joint 2) is measured by an encoder; : Wrist joint push rod length (corresponding to joint 3), measured by encoder.

[0035] Step 3: Based on the pre-stored geometric parameters of the robotic arm links, the actual linear displacement of each push rod is converted into the rotation angle of the corresponding joint through a chain derivation of the cosine theorem of multi-level triangles, thus obtaining the current joint configuration.

[0036] Understandably, during system initialization, multiple fixed angles are pre-calculated based on the lengths of fixed members that do not change with the push rod. These fixed angles are obtained by using the cosine theorem from the lengths of the three members that form the corresponding fixed triangle.

[0037] Specifically, using the Law of Cosines to find angles: Given the three sides of a triangle... ,beg opposite angle :

[0038] Recorded as .

[0039] Finding a side using the Law of Cosines: Given two sides and its included angle Find the opposite side : .

[0040] During system initialization, based on the length of the fixed link that does not change with the push rod, the four fixed angles of the robot mechanism are pre-calculated:

[0041]

[0042]

[0043]

[0044] Based on the linear displacement of the push rod corresponding to each joint, and combined with a fixed angle, the rotation angle of each joint is solved using the triangle cosine theorem.

[0045] The derivation of each joint angle is as follows: (1) Joint 1 ( (upper arm): Due to the length of the push rod Starting from here, find the angle using a triangle in one step:

[0046]

[0047]

[0048] (2) Joint 2 ( (forearm) Due to the length of the push rod To begin, we need to solve for the intermediate angle in 6 steps and the auxiliary edge in 1 step:

[0049]

[0050]

[0051]

[0052]

[0053]

[0054]

[0055]

[0056]

[0057] (3) Joint 3 ( (wrist joint) Due to the length of the push rod Starting from this point, the solution is obtained through 4 steps to find the intermediate angle and 1 step to find the auxiliary edge:

[0058]

[0059]

[0060]

[0061]

[0062]

[0063]

[0064] in,( , , ) represents the current angle of each joint, expressed in ( , , ) indicates the current configuration of the mechanism.

[0065] Step 4: Sample within the contact angle constraint range of the end effector. Based on the target anchor point position and target contact angle, combined with vertical constraints and joint limit constraints, generate a set of feasible inverse kinematic target configurations that satisfy all constraints.

[0066] Understandably, within the preset end-fitting angle range N sampling angle values ​​are uniformly sampled within the interior (e.g., N = 501).

[0067] For each sampled angle value, the direction of the end link is determined based on the target anchor point position, and the length of the end link is offset in the opposite direction from the target anchor point position along the end link direction to obtain the target position of the wrist joint.

[0068] Specifically, for each sample (The kth sampling angle value is...) (Target value) (1) By Rotate the target direction unit vector to obtain the end link direction :

[0069] in,( , ( ) represents the target attitude direction, and the target fitting direction is calculated based on step 1. Calculated.

[0070] (2) Calculate the position of the wrist joint ,in, This refers to the target anchor point position, calculated based on step 1. It is calculated from... This refers to the length of the end link.

[0071] (3) Constrained by the vertical coordinates of the target position of the wrist joint, Solve , It refers to the vertical component (y-coordinate) of the forearm joint position. This refers to the vertical component of the wrist joint position. The length of the upper arm joint. This refers to the length of the forearm joint.

[0072] (4) If If there is no solution, skip the current sampling angle value; otherwise... This produces two sets of solutions. This refers to the horizontal offset of the end of the upper arm (joint 2).

[0073] (5) Constraint verification of whether the angle of each joint is within the physical limit: Joint 2 angle : arrive The clockwise angle, to verify ; Joint 1 angle ,verify .

[0074] For all solutions that pass the constraint verification, sort them by base offset and output the complete solution set (including joint angles, joint positions, link directions, etc.).

[0075] Its output can be represented as: text Solution set = [ { "Joint Angles": (θ1, θ2, θ3), / / Target angles of the three joints "Joint Position": { / / Coordinates of each joint in space} "J1": (x1, y1), / / Joint 1 (shoulder) position "J2": (x2, y2), / / Joint 2 (elbow) position "J3": (x3, y3), / / Joint 3 (wrist) position "J4": (x4, y4) / / Joint 4 (end) position = P_anchor }, "Link Direction": { / / Direction vector of each link} "d1": (d1x, d1y), / / Arm direction "d2": (d2x, d2y), / / forearm direction "d3": (d3x, d3y) / / Direction of the end link }, "Base offset": x_base, / / Sorting criteria "Configuration identifier": "Elbow up / down" / / Distinguish between positive and negative solutions }, { ...}, / / Second feasible solution { ...}, / / The third feasible solution ... Step 5: Based on the current joint configuration and the set of feasible inverse kinematics target configurations, plan a collision-free motion trajectory from the current configuration to the target configuration under the constraints of the collision model.

[0076] Understandably, this step, based on the current joint configuration and various feasible target configurations, plans a collision-free motion trajectory under collision model constraints. The specific steps are as follows: 1. Construction of robot and scene models: 1.1 Load the Unified Robot Description Format (URDF) kinematic chain model for the robotic arm. The URDF kinematic chain model defines a kinematic chain containing 6 joints: world2base (prism joint): the base translates along the X-axis; zhuantai (rotation joint): the turntable rotates; guanjie1, guanjie2, guanjie3 (rotation joints): 3 arm joints; xuanfu (prism joint): the end-effector suspension axis (the working head extends out); each link is accompanied by a 3D collision mesh in STL format.

[0077] 1.2 Define the Semantic Robot Description Format (SRDF) planning group configuration, where the SRDF planning group configuration specifies the subset of joints participating in the planning (e.g., world2base, guanjie1, guanjie2, guanjie3) and defines self-collision exclusion pairs.

[0078] 1.3 Obstacle Scene Construction: Based on the target panel component's position and normal vector, model the target panel component as a cuboid collider and add it to the scene. Align the obstacle orientation by converting the normal vector to a quaternion.

[0079] This formula converts the normal vector (3D orientation) of the target panel into a quaternion (rotation representation) for correctly oriented obstacles (cubes) in a 3D scene.

[0080] in, The output quaternion represents the rotational attitude of the obstacle (panel) in three-dimensional space; Represents the z-axis unit vector, the "up" direction in the world coordinate system, and is the default orientation; This represents the unit vector of the target panel normal direction, obtained from upstream sensing, pointing towards the panel surface; This represents the quaternion normalization function, ensuring that the output quaternions are unit quaternions and avoiding numerical distortion.

[0081] 2. BiRRT Multi-Object Collision-Free Planning: The BiRRT (Bidirectional Fast Expanding Random Tree) algorithm based on the Pinocchio rigid body dynamics library is used for collision-free motion trajectory planning: 2.1 Planning Space: This is carried out in the joint space of the planning group's joint subset.

[0082] 2.2 Multi-target input: The multiple sets of feasible inverse solutions obtained in step 4 are used as candidate target configuration arrays. BiRRT expands to all targets simultaneously and selects the optimal path that is reached first.

[0083] 2.3 Collision Detection: Each time a tree node expands, the Pinocchio collision detection engine is called to determine whether the current configuration collides with scene obstacles based on the STL collision mesh.

[0084] 2.4 Trajectory Post-processing: Perform time-optimal smoothing on the joint space path under velocity and acceleration constraints.

[0085] The planning of collision-free motion trajectories is divided into two phases, as detailed below: Phase 1: Based on the 3D collision model, the Bidirectional Rapidly Expanding Random Tree (BiRRT) algorithm is used to plan the collision-free trajectories of all joints except the end effector suspension axis from the current configuration to the target configuration. The end effector suspension axis remains in its initial position, and each joint of the robotic arm is controlled to execute the corresponding collision-free motion trajectory. Plan the collision-free trajectory of `world2base, guanjie1, guanjie2, guanjie3` from the current configuration to the target configuration; `xuanfu` (floating axis) remains unchanged at its initial value; Fill the planned subspace trajectory back into the full joint space.

[0086] Phase Two: After each joint reaches the target configuration, the end effector of the robotic arm extends gradually along its axis, and a collision detection is performed after each extension step. If no collision is detected, the extension continues; if a collision is detected, the extension is stopped and the arm retracts to the previous safe position, thus achieving adaptive fitting between the end effector and the target panel component. Based on the phase one endpoint configuration, with a fixed step size Incrementing xuanfu value, where, This represents the single-step elongation of the suspension shaft. To limit the speed of the suspension shaft, This represents the time interval for each iteration.

[0087] Collision detection is performed at each step: If setting xuanfu directly to the upper limit value will not cause a collision, then the entire process will proceed to the upper limit. If a collision occurs, the vehicle will advance gradually, and upon collision, it will retreat to the previous safe value and terminate. This strategy ensures that the fitting head fits as closely as possible to the target panel surface, with the collision serving as a "positioning" signal.

[0088] 4. 3D visualization: Real-time rendering of the entire robotic arm movement process via the Meshcat engine, supporting: Initial configuration shown; Obstacle visualization (target panel plane + custom obstacles); Joint space trajectory animation playback.

[0089] Step 6: Drive each joint of the robotic arm to move along the collision-free motion trajectory to the target configuration, and control the end-effector to advance towards the surface of the target panel component until a safe fitting position is reached.

[0090] Understandably, after planning the collision-free motion trajectory from the current configuration to the target configuration in step 5 above, the robotic arm's joints are driven to move along the collision-free motion trajectory to the target configuration. Specifically, this includes: 1. Independent PID control for each joint: Independent PID controllers are set up for the upper arm joint, forearm joint, and wrist joint, and motor speed commands are generated based on the deviation between the target angle and the current angle of each joint.

[0091] in, Let be the velocity of the i-th joint. For the proportional gain of the i-th joint, Let i be the target angle of the i-th joint. The current angle of the i-th joint is given, and the speed output is limited to the maximum allowable speed range of each axis.

[0092] 2. Gaussian velocity decay coordination strategy: To avoid excessive end-effector trajectory deviation due to asynchronous joint positioning, a global velocity decay factor based on a Gaussian function is introduced. When the end-effector angle deviates significantly, reduce the speed of the upper and lower arms to allow the wrist joint time to catch up.

[0093] in, The target angle for the wrist joint. This represents the current angle of the wrist joint. The standard deviation of the Gaussian function is given, and the actual speed command for each joint is... (Except for the wrist joint, the speed of the wrist joint remains unchanged).

[0094] In scenarios where the long side of the panel component has a large scale, this strategy can be switched to an attenuation mode based on the end orientation angle deviation:

[0095] in, The target direction angle at the end. This represents the current direction angle at the end point.

[0096] 3. Judgment of multi-joint coordinated positioning: When all joint angle deviations simultaneously meet their respective positioning thresholds, the robotic arm is determined to be in position, triggering the end effector to begin panel surface machining operations. The triggering condition is that the following conditions are met simultaneously:

[0097]

[0098]

[0099] Typical threshold:

[0100] See Figure 3 This paper illustrates a robotic arm fitting operation control system based on pushrod encoder calculation and obstacle avoidance planning according to an embodiment of the present invention, comprising: The acquisition module 301 is used to acquire the three-dimensional target posture of the target panel component surface, project the three-dimensional target posture onto the working plane of the robotic arm, and generate the target anchor point position and target fitting angle in the two-dimensional plane. The conversion module 302 is used to read the encoder values ​​of the push rod motors driving each joint of the robotic arm in real time and convert them into the actual linear displacement of each push rod. Based on the pre-stored geometric parameters of the robotic arm links, the actual linear displacement of each push rod is converted into the rotation angle of the corresponding joint through the chain derivation of the cosine theorem of multi-level triangles to obtain the current joint configuration. The generation module 303 is used to sample within the contact angle constraint range of the end effector, and combine the vertical constraint and joint limit constraint to generate a set of feasible inverse kinematic target configurations that satisfy all constraints. Planning module 304 is used to plan a collision-free motion trajectory from the current configuration to the target configuration under the constraints of the collision model, based on the current joint configuration and the set of feasible inverse kinematics target configurations. The execution module 305 is used to drive each joint of the robotic arm to move along the collision-free motion trajectory to the target configuration, and control the end-effector to advance towards the surface of the target panel component until a safe fitting position is reached.

[0101] It is understood that the robotic arm fitting operation control system based on push rod encoder calculation and obstacle avoidance planning provided by the present invention corresponds to the robotic arm fitting operation control method based on push rod encoder calculation and obstacle avoidance planning provided in the foregoing embodiments. The relevant technical features of the robotic arm fitting operation control system based on push rod encoder calculation and obstacle avoidance planning can be referred to the relevant technical features of the robotic arm fitting operation control method based on push rod encoder calculation and obstacle avoidance planning, and will not be repeated here.

[0102] The present invention provides a robotic arm fitting operation control method and system based on push rod encoder calculation and obstacle avoidance planning, which has the following beneficial effects: (1) A complete multi-stage linkage geometric solution model was established for a 3R robotic arm driven by a push rod motor. The solution of each joint angle involves 3 to 7 steps of intermediate triangle derivation and 0 to 1 steps of auxiliary side calculation. Through the chain application of the cosine theorem, the linear displacement of the three push rods is converted into joint rotation angles step by step. This method only relies on the geometric constant parameters of the rods on the mechanism drawing, requires no empirical calibration, and has analytical accuracy and good portability.

[0103] 2. This paper proposes a method that uses uniformly dense sampling (e.g., 501 points) within the end effector's contact angle constraint range. Combining the geometric solution of the vertical constraint and the step-by-step verification of all joint limits, it iterates through all feasible inverse kinematic solutions that satisfy the constraints. Compared to traditional analytical methods that only provide 1-2 sets of solutions, this method can output tens to hundreds of feasible solutions, providing rich candidate targets for subsequent obstacle avoidance motion planning and significantly improving the planning success rate.

[0104] 3. A two-stage decoupling strategy of "arm joint BiRRT planning + gradual advancement of suspension axis" is proposed: Phase 1: Utilize BiRRT to search for the optimal collision-free path in a multi-objective (fully feasible inverse solution set) environment, making full use of the advantages of multiple solutions; Phase Two: After reaching the target configuration, the suspension shaft is advanced separately, and collision detection is used as the "positioning" signal to achieve adaptive bonding between the bonding work head and the surface of the target panel component; This strategy decouples the two objectives of "safe positioning" and "precise fit," ensuring both the safety of the movement and achieving adaptive fit at the end effector.

[0105] 4. Gaussian velocity decay multi-joint coordinated control strategy: Introducing a global velocity decay factor based on a Gaussian function, the velocity of each joint is dynamically adjusted according to the deviation of the end effector attitude angle (or orientation angle). When the end effector is not aligned, the velocity of the joints that are close to being in position is reduced to avoid end effector trajectory deviation caused by asynchronous positioning of the joints, thus achieving smooth and coordinated multi-joint motion.

[0106] It should be noted that the descriptions of each embodiment in the above embodiments have different focuses. For parts that are not described in detail in a certain embodiment, please refer to the relevant descriptions in other embodiments.

[0107] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0108] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded computer, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0109] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0110] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0111] Although preferred embodiments of the invention have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the appended claims are intended to be interpreted as including both the preferred embodiments and all changes and modifications falling within the scope of the invention.

[0112] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from its spirit and scope. Therefore, if these modifications and variations fall within the scope of the claims of this invention and their equivalents, this invention also intends to include these modifications and variations.

Claims

1. A robotic arm fitting operation control method based on pushrod encoder calculation and obstacle avoidance planning, characterized in that, include: The three-dimensional target pose of the target panel component surface is obtained, and the three-dimensional target pose is projected onto the working plane of the robotic arm to generate the target anchor point position and target fitting angle in the two-dimensional plane. The encoder values ​​of the push rod motors driving each joint of the robotic arm are read in real time and converted into the actual linear displacement of each push rod. Based on the pre-stored geometric parameters of the robotic arm links, the actual linear displacement of each push rod is converted into the rotation angle of the corresponding joint through the chain derivation of the cosine theorem of multi-level triangles, thus obtaining the current joint configuration. Sampling is performed within the contact angle constraint range of the end effector. Based on the target anchor point position and target contact angle, combined with vertical constraint conditions and joint limit constraints, a set of feasible inverse kinematic target configurations that satisfy all constraints is generated. Based on the current joint configuration and the set of feasible inverse kinematics target configurations, a collision-free motion trajectory from the current joint configuration to the target configuration is planned under the constraints of the collision model. The robotic arm joints are driven to move along the collision-free motion trajectory to the target configuration, and the end-effector is controlled to advance towards the surface of the target panel component until a safe fitting position is reached.

2. The method according to claim 1, characterized in that, The three-dimensional target pose includes anchor point position and normal vector direction. Projecting the three-dimensional target pose onto the working plane of the robotic arm generates the target anchor point position and target fitting angle in a two-dimensional plane, including: Define a coordinate system for the turntable working plane with the turntable rotation center as the origin, the turntable orientation direction as the X-axis, and the vertical upward direction as the Z-axis; The reference points at the anchor point position and in the direction of the normal vector are projected onto the coordinate system of the turntable working plane to obtain the two-dimensional anchor point coordinates and the two-dimensional normal point coordinates. The target fitting angle is calculated based on the two-dimensional anchor point coordinates and the two-dimensional normal point coordinates.

3. The method according to claim 1, characterized in that, The robotic arm is a 3-DOF robotic arm comprising an upper arm joint, a forearm joint, and a wrist joint, each joint being independently driven by a first push rod, a second push rod, and a third push rod, respectively. Based on pre-stored robotic arm link geometry parameters, and through a chain derivation using the cosine theorem of multi-level triangles, the actual linear displacement of each push rod is converted into the rotation angle of the corresponding joint, resulting in the current joint configuration, including: During system initialization, multiple fixed angles are pre-calculated based on the length of fixed members that does not change with the push rod. These fixed angles are obtained by using the law of cosines from the lengths of the three members that form the corresponding fixed triangle. For the upper arm joint, based on the linear displacement of the first push rod, the intermediate angle is solved by the one-step triangle cosine theorem, and combined with the pre-calculated fixed angle, the rotation angle of the upper arm joint is calculated. For the forearm joint, based on the linear displacement of the second push rod, the rotation angle of the forearm joint is obtained by solving the triangle cosine theorem in six steps and calculating the auxiliary side in one step. For the wrist joint, based on the linear displacement of the third push rod, the wrist joint rotation angle is obtained by solving the triangle cosine theorem in four steps and calculating the auxiliary side in one step. The current joint configuration is obtained based on the rotation angles of the upper arm joint, forearm joint, and wrist joint. The auxiliary side is obtained by using the law of cosines to find the opposite side from two known sides and their included angle in a pre-established triangle model; the intermediate angle is obtained by using the law of cosines to find the angle from three known sides in a pre-established triangle model.

4. The method according to claim 3, characterized in that, The sampling is performed within the contact angle constraint range of the end effector. Based on the target anchor point position and target contact angle, combined with vertical constraints and joint limit constraints, a set of feasible inverse kinematic target configurations that satisfy all constraints is generated, including: N sampling angle values ​​are uniformly sampled within a preset end-fitting angle range, where N is a positive integer; For each sampled angle value, the direction of the end link is determined based on the target anchor point position, and the length of the end link is offset in the opposite direction from the target anchor point position along the end link direction to obtain the target position of the wrist joint; Based on the vertical coordinate constraint of the target position of the wrist joint, the combination of joint angles of the upper arm joint and the forearm joint is solved. When the target position of the wrist joint exceeds the maximum extension range of the upper arm joint and the forearm joint, it is determined that there is no solution and the current sampled angle value is skipped; otherwise, the current sampled angle value is retained. For each set of joint angles obtained by the solution, verify whether they meet the angle limit constraints of the corresponding joint. All joint angles that have passed the joint limit verification are arranged according to a preset sorting rule to form the set of feasible inverse kinematic target configurations.

5. The method according to claim 1, characterized in that, The step of planning a collision-free motion trajectory under collision model constraints based on the current joint configuration and the set of feasible inverse kinematics target configurations includes: The loading robot arm uses the Unified Robot Description Format (URDF) kinematic chain model and the Semantic Robot Description Format (SRDF) planning group configuration. The URDF kinematic chain model defines the three-dimensional collision mesh attached to each joint type and link, and the SRDF planning group configuration specifies the subset of joints participating in the planning and self-collision exclusion pairs. Based on the anchor point position and normal vector direction of the target panel component, the target panel component is modeled as a cuboid collision obstacle and added to the three-dimensional collision model; The Bi-directional fast expanding random tree (BiRRT) algorithm is used to search in the joint space of the joint subset participating in the planning, using the set of feasible inverse kinematic target configurations as multiple candidate targets, and simultaneously expanding tree nodes to each candidate target, selecting the first optimal path reached as the collision-free motion trajectory. Specifically, each time a tree node expands, the collision detection engine is invoked to determine whether the current joint configuration collides with an obstacle in the scene based on the three-dimensional collision mesh.

6. The method according to claim 5, characterized in that, The planned collision-free motion trajectory includes: In the first stage, based on the three-dimensional collision model, the BiRRT algorithm is used to plan the collision-free trajectories of the joints, except for the end-effector suspension axis, from the current configuration to the target configuration. The end-effector suspension axis remains in its initial position, and the joints of the robotic arm are controlled to execute the corresponding collision-free motion trajectory. In Phase Two, after each joint reaches the target configuration, the end effector of the robotic arm extends gradually along its axis, and a collision detection is performed after each extension. If there is no collision, the extension continues; if a collision is detected, the extension is stopped and the arm retracts to the previous safe position, so as to achieve adaptive fitting between the end effector and the target panel component.

7. The method according to claim 1, characterized in that, The joints of the driven robotic arm move along the collision-free motion trajectory to the target configuration, including: During the execution of the collision-free motion trajectory by each joint, an independent PID controller is used for closed-loop control of each joint, and the speed of each joint is dynamically adjusted according to the end-effector attitude deviation to achieve coordinated movement of multiple joints.

8. The method according to claim 7, characterized in that, The method of using independent PID controllers for closed-loop control of each joint and dynamically adjusting the speed of each joint based on the end-effector attitude deviation to achieve coordinated movement of multiple joints includes: Each joint is set with an independent PID controller to calculate the deviation between the target angle and the current angle of the end effector, and generate speed commands for each joint based on the deviation. A global velocity attenuation factor is calculated based on the Gaussian function and the deviation. When the end attitude angle or orientation angle deviation is large, the speed of the joints that are close to being in position is reduced based on the global speed attenuation factor, so that all joints can be coordinated to be in position.

9. The method according to claim 8, characterized in that, When the deviation between the actual angle and the target angle of all joints is less than their respective preset positioning thresholds, the robotic arm is determined to be in position and the end effector is triggered to start performing panel surface processing operations.

10. A robotic arm fitting operation control system based on push rod encoder calculation and obstacle avoidance planning, characterized in that, include: The acquisition module is used to acquire the three-dimensional target posture of the target panel component surface, project the three-dimensional target posture onto the working plane of the robotic arm, and generate the target anchor point position and target fitting angle in the two-dimensional plane. The conversion module is used to read the encoder values ​​of the push rod motors driving each joint of the robotic arm in real time and convert them into the actual linear displacement of each push rod. Based on the pre-stored geometric parameters of the robotic arm links, the actual linear displacement of each push rod is converted into the rotation angle of the corresponding joint through the chain derivation of the cosine theorem of multi-level triangles, thus obtaining the current joint configuration. The generation module is used to sample within the contact angle constraint range of the end effector. Based on the target anchor point position and target contact angle, combined with vertical constraints and joint limit constraints, it generates a set of feasible inverse kinematic target configurations that satisfy all constraints. The planning module is used to plan a collision-free motion trajectory from the current configuration to the target configuration under the constraints of the collision model, based on the current joint configuration and the set of feasible inverse kinematics target configurations. The execution module is used to drive each joint of the robotic arm to move along the collision-free motion trajectory to the target configuration, and control the end-effector to advance towards the surface of the target panel component until a safe fitting position is reached.

Images