A mobile manipulator dynamic grasping positioning compensation method based on IMU and hand-eye system

Abstract

This invention discloses a dynamic grasping and positioning compensation method for a mobile robotic arm based on an IMU and a hand-eye system. This method establishes a precise pose transfer relationship through "robotic arm and camera extrinsic parameter calibration, and cart and robotic arm coordinate system calibration," reusing the vehicle-mounted IMU and the depth camera at the robotic arm's end (facing the target object). During cart movement, the depth camera detects the target object's distance in real time. When the target object enters the preset optimal grasping range, the method fuses IMU dynamic offset data and initial visual positioning data in real time, combined with a rigid connection structure to ensure structural stability and compensate for pose deviations caused by cart movement. This invention eliminates the need for external positioning equipment, significantly reducing hardware costs compared to laser tracker solutions. In uniform speed or micro-vibration scenarios of 0.1–0.3 m / s, it can be used for automatic loading and unloading in vapor chamber processes, with a robotic arm end-effector positioning deviation ≤0.8 mm and a dynamic grasping success rate of ≥90% for small targets. It is suitable for dynamic scenarios without infrastructure, such as laboratory sample transport and desktop workpiece grasping.

Description

Technical Field

[0001] This invention relates to the field of mobile robot grasping control technology, and in particular to a dynamic grasping and positioning compensation method for a mobile robotic arm based on an IMU and a hand-eye system. Background Technology

[0002] Dynamic grasping and positioning technology of mobile robotic arms is one of the core technologies in the field of mobile robotics. It is widely used in scenarios such as laboratory sample transfer and desktop small workpiece grasping. These scenarios have extremely high requirements for the positioning accuracy of dynamic grasping, especially for small targets with a diameter of 8-10mm, where the grasping tolerance is usually required to be <1mm. Although there have been many attempts to improve the dynamic grasping and positioning of mobile robotic arms, there are still many core problems that are difficult to solve. CN202310281559.5 discloses a mobile grasping scheme based on "eye-on-hand" vision. It uses a depth camera at the end of the robotic arm to collect the three-dimensional coordinates of the target and plan the grasping path. However, this design is only based on the initial positioning data when the vehicle is stationary, and does not take into account the dynamic drift of the platform base posture during the vehicle's movement. This causes the deviation between the actual position of the robotic arm end and the target to accumulate as the movement distance increases, resulting in a grasping success rate of less than 50% for sample tubes with a diameter of 8-10mm. To address this deficiency, CN201710668913.4 attempted to introduce a wheeled odometer to correct the gripping positioning error. However, in scenarios involving vehicle acceleration, obstacle crossing, or uneven ground, the wheels are prone to slippage, causing the odometer positioning error to accumulate to over 5mm, which is far from meeting the high-precision gripping requirements of small workpieces. CN201810023432.2 proposed a fusion positioning mode combining a laser tracker and a vision sensor. While this improves positioning accuracy, the laser tracker requires the deployment of at least two external base stations, with a single system costing over 120,000 yuan. Furthermore, the base stations need to be pre-installed in fixed locations, making it difficult to flexibly adapt to small application scenarios without pre-installed infrastructure, thus creating a contradiction between "accuracy improvement and cost control." In addition, although CN201810737223.4 recognized the impact of the connection structure on positioning and proposed an optimization scheme, it still adopted a flexible connection method with ordinary bolts and elastic washers. When the car moves, the relative displacement between the robotic arm and the car base can reach 1.2-1.5mm. This displacement will directly amplify the pose error collected by the sensor, resulting in a secondary deterioration of the end-positioning accuracy, forming a vicious cycle of "sensor compensation - structural error superposition". In summary, the existing technology always has three core problems: (1) During dynamic movement, the platform pose drift is ignored or the compensation method is easily affected by environmental interference, and the static positioning logic cannot be adapted to the car movement scenario; (2) High-precision solutions rely on high-cost external equipment, and low-cost solutions have insufficient accuracy, making it difficult to balance accuracy and economy; (3) There is a lack of dedicated rigid mechanical connection structure to ensure the reliability of pose transmission. The structural error further amplifies the positioning deviation, and ultimately cannot meet the precise grasping requirements of small fixed targets. Summary of the Invention

[0003] The main objective of this invention is to overcome the shortcomings and deficiencies of existing technologies. This invention provides a dynamic grasping and positioning compensation method for a mobile robotic arm based on an IMU and a hand-eye system. The purpose is to provide a method that eliminates the need for external positioning equipment, reuses only the vehicle-mounted IMU and the depth camera at the end of the robotic arm, and achieves sub-millimeter-level dynamic positioning and grasping of the target object by means of "precise calibration of dual coordinate systems + IMU and vision collaborative compensation + rigid connection guarantee". When the depth camera detects that the target object has entered the optimal grasping range during the movement of the vehicle, it updates the target pose and generates robotic arm motion commands based on IMU data within a period of ≤10ms to correct the pose deviation caused by the movement of the vehicle. This overcomes the defects of existing technologies such as "no calibration, soft structure, and low accuracy".

[0004] To achieve the above objectives, the present invention adopts the following technical solution:

[0005] This invention provides a dynamic grasping and positioning compensation method for a mobile robotic arm based on an IMU and a hand-eye system, comprising the following steps:

[0006] S1. Rigid connection and vision system construction: The six-degree-of-freedom robotic arm is fixed to the base on the top of the mobile trolley through a rigid flange. A depth camera facing the target object is rigidly installed at the end of the robotic arm to form a hand-eye system.

[0007] S2. Dual-coordinate system precise calibration: A checkerboard calibration board is used to control the robotic arm to drive the camera to take multiple sets of images in various postures. The external parameter matrix of the camera coordinate system and the robotic arm end coordinate system is solved by the hand-eye calibration algorithm. The trolley is controlled to move at a constant speed along a preset straight trajectory on the ground, and the pose data of the trolley base coordinate system and the robotic arm coordinate system are collected to establish a fixed transformation relationship between the two.

[0008] S3. IMU Integration and Calibration: Inertial Measurement Unit (IMU) is integrated on the vehicle. The IMU coordinate system is aligned and calibrated with the vehicle's motion coordinate system so that the IMU's X-axis is consistent with the vehicle's forward direction and the Z-axis is consistent with the direction of gravity. The IMU sampling frequency is not less than 100Hz.

[0009] S4. Target Detection and Compensation Trigger: Control the car to move towards the target object. The depth camera continuously captures images of the target object at a set frame rate. The target center pixel coordinates are extracted using a target detection algorithm. Combined with the camera intrinsic parameter matrix, the pixel coordinates are converted into three-dimensional coordinates in the camera coordinate system, and the real-time distance between the target and the camera is calculated. When the distance is within the optimal grasping range, the dynamic positioning compensation process is triggered, and the compensation trigger moment is recorded. ;

[0010] S5. Initial pose acquisition: In At any given moment, the coordinates of the target in the camera coordinate system are converted to the coordinates in the robot arm end-effector coordinate system by combining the extrinsic parameter matrices of the camera and the robot arm end-effector coordinate system. Then, the coordinates are converted to the coordinates in the robot arm base coordinate system by using the forward kinematics formula of the robot arm. Finally, the initial pose of the target in the car coordinate system is calculated by back-calculating based on the fixed transformation relationship between the car and the robot arm.

[0011] S6. Dynamic Offset Sensing and Pose Correction: As the vehicle continues to move, the IMU collects angular acceleration and linear acceleration data in real time, and calculates the vehicle's position relative to the ground using a Kalman filter algorithm. The pose changes ΔX, ΔY, and Δθ at each moment are combined with the fixed transformation relationship between the trolley and the robotic arm to superimpose the pose changes onto the initial pose of the target, thus obtaining the corrected pose of the target in the current coordinate system of the robotic arm base.

[0012] S7. Dynamic Grasping Execution: The target angles of each joint are calculated based on the corrected pose using the inverse kinematics formula of the robotic arm. The robotic arm controller sends joint target angle commands with an instruction update cycle of ≤10ms, driving the robotic arm to adjust in real time. When the end effector of the robotic arm detects a contact force ≥1~5N with the target object, it triggers the gripper closing command, completing the target grasping during the continuous movement of the trolley, and the single compensation cycle delay is ≤10ms.

[0013] As a preferred technical solution, in step S1, the rigid flange is an integrally formed aluminum alloy flange, and the relative displacement deformation between the rigid flange and the base is within 0.1mm.

[0014] As a preferred technical solution, in step S2, the robotic arm drives the camera to take pictures in multiple poses, covering the pitch angle from -30° to +30° and the rotation angle from 0 to 360°, and the calibration board corner point recognition rate of each group of images is ≥80%.

[0015] As a preferred technical solution, the IMU uses the Kalman filter algorithm to calculate the pose change. By dynamically adjusting the process noise covariance matrix Q and the observation noise covariance matrix R, the pose calculation accuracy under uneven ground and vehicle acceleration scenarios is improved. The angular acceleration measurement error is <0.01 rad / s², the linear acceleration measurement error is <0.02 m / s², and the IMU sampling rate is 100-200 Hz.

[0016] As a preferred technical solution, in step S4, the target center pixel coordinates are extracted using a target detection algorithm, and the pixel coordinates are converted into three-dimensional coordinates in the camera coordinate system by combining the camera intrinsic parameter matrix, and the real-time distance between the target and the camera is calculated. Specifically:

[0017] The target is identified by a deep learning-based target detection algorithm, and the center pixel coordinates (u, v) of the target are extracted. Combined with the camera intrinsic parameter matrix K, the pixel coordinates are converted into three-dimensional coordinates in the camera coordinate system using formula (1). ),

[0018] (1)

[0019] Among them, the camera intrinsic parameter matrix , For camera focal length, Z_cam represents the coordinates of the primary point, obtained from the camera's factory calibration; Z_cam represents the depth value of the target point in the camera's depth map.

[0020] Calculate the real-time distance between the target and the camera. When D is within the preset optimal capture range, a dynamic compensation process is triggered.

[0021] As a preferred technical solution, step S5 specifically includes:

[0022] At the time of compensation triggering Combining the extrinsic parameter matrices (R, T) of the camera and the end effector of the robotic arm, the coordinates of the target in the camera coordinate system are obtained using formula (2). Convert the coordinates to the coordinates in the robotic arm's end effector coordinate system. );

[0023] (2)

[0024] Then, by using the forward kinematics formula (3) of the robotic arm, combined with The angles of each joint of the robotic arm at all times Calculate the coordinates in the robot arm base coordinate system. );

[0025] (3)

[0026] Where f(x) is the forward kinematics function of the robotic arm, which is derived based on the DH parameter method. The DH parameters are provided by the robotic arm manufacturer or calibrated by actual measurement.

[0027] Finally, based on the fixed conversion relationship between the trolley and the robotic arm: , The initial pose of the target in the car coordinate system is obtained by inverse calculation. .

[0028] As a preferred technical solution, the filtering process in step S6 is as follows:

[0029] Equations of state: Wherein, the state vector x and y represent translational displacements, and θ represents the rotation angle. ω is the linear velocity, ω_z is the angular velocity; A is the state transition matrix, B is the input matrix. The acceleration input is collected by the IMU, and w_k is the process noise;

[0030] Observation equation: Wherein, the observation vector Z_k is composed of the IMU measured velocity and angular velocity, H is the observation matrix, and v_k is the observation noise;

[0031] Filtering update: Through iterative prediction and update steps, the optimal pose change is output. , , , for The car's position at all times.

[0032] As a preferred technical solution, in step S6, the pose change is superimposed on the initial pose of the target based on the fixed transformation relationship between the trolley and the robotic arm to obtain the corrected pose of the target in the current coordinate system of the robotic arm base, specifically as follows:

[0033] Combining the conversion relationship of the "cart-robotic arm" calibration, the pose change is superimposed on the initial pose of the target using formula (4) to obtain the corrected pose of the target in the current robotic arm base coordinate system. );

[0034]

[0035] (4)

[0036] ;

[0037] Then, using the inverse kinematics formula (5) for the robotic arm, the target angles of each joint are calculated. ), generate motion commands for the robotic arm's end effector;

[0038] ; (5)

[0039] Among them, f -1 (x) is the inverse kinematics function of the robotic arm, which is solved by numerical iteration to ensure the uniqueness and real-time performance of the solution.

[0040] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0041] (1) Significantly improved dynamic grasping accuracy: Under the condition of the trolley moving at a constant speed of 0.1–0.3 m / s or crossing a 5 mm obstacle with micro-vibration, when the target object enters the optimal grasping range, the positioning deviation of the end of the robotic arm is ≤0.8 mm. This is mainly due to the dual calibration to ensure that the coordinate transformation is distortion-free and the IMU to compensate for the trolley's motion offset in real time.

[0042] (2) Significantly reduced hardware costs: It eliminates the need for dedicated high-cost equipment such as laser trackers and external motion capture systems, and only relies on the inertial measurement unit (IMU) built into the mobile platform and the depth camera already installed at the end of the robotic arm to achieve sensor reuse and avoid additional hardware investment.

[0043] (3) Strong scene adaptability: rigid connection + no external equipment dependence, suitable for laboratory sample transportation, desktop small workpiece grasping and other scenarios without infrastructure, with a dynamic grasping success rate of ≥90% for small targets with a diameter of 8-10mm;

[0044] (4) High robustness: IMU-visual collaborative compensation can resist the interference of uneven ground and the car's acceleration and slippage. Dual calibration + rigid connection ensures the stability of the pose transfer chain of "IMU→car→robotic arm→camera" and avoids the sudden drop in accuracy caused by the failure of a single sensor. Attached Figure Description

[0045] To more clearly illustrate the technical solutions in the embodiments of this application, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0046] Figure 1 This is a schematic diagram of the overall structure of the mobile robotic arm dynamic grasping and positioning compensation device described in this invention;

[0047] Figure 2 This is a flowchart illustrating the calibration process of the "robotic arm-camera" of the present invention.

[0048] Figure 3 This is a schematic diagram of the calibration process for the "cart-robotic arm" of the present invention;

[0049] Figure 4 This is a flowchart of the dynamic positioning compensation method described in this invention (including the "target distance judgment → compensation trigger" step).

[0050] Explanation of reference numerals in the attached figures:

[0051] 1-Trolley; 2-Rigid flange; 3-Anti-loosening bolt; 4-Robot arm; 5-Depth camera; 6-Inertial measurement unit; 7-Trolley main control board; 8-Origin of robot arm base coordinate system; 9-Origin of trolley base coordinate system. Detailed Implementation

[0052] To enable those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are merely some embodiments of the present application, and not all embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of the present application without creative effort are within the scope of protection of the present application.

[0053] In this application, the reference to "embodiment" means that a specific feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of this application. The appearance of this phrase in various places throughout the specification does not necessarily refer to the same embodiment, nor is it a mutually exclusive, independent, or alternative embodiment. It will be explicitly and implicitly understood by those skilled in the art that the embodiments described in this application can be combined with other embodiments.

[0054] This embodiment provides a dynamic grasping and positioning compensation method for a mobile robotic arm based on an IMU and a hand-eye system, including the following steps:

[0055] S1. Rigid connection and vision system construction: The six-degree-of-freedom robotic arm is fixed to the base on the top of the mobile trolley through a rigid flange. A depth camera facing the target object is rigidly installed at the end of the robotic arm to form a hand-eye system.

[0056] like Figure 1 As shown, the overall structure of the mobile robotic arm dynamic grasping and positioning compensation device used in this embodiment includes: a trolley 1, a rigid flange 2, anti-loosening bolts 3, a robotic arm 4, a depth camera 5, an inertial measurement unit 6, and a trolley main control board 7. The robotic arm 4 is fixed to the trolley 1 via the rigid flange 2, and the rigid flange 2 is locked by the anti-loosening bolts 3. The depth camera 5 is fixed to the end of the robotic arm 4 via a bracket, and the inertial measurement unit 6 (IMU) is installed at the center of the trolley chassis. 2 The C-bus communicates with the main control board 7 of the vehicle.

[0057] Furthermore, the assembly relationship of the overall structure in this embodiment is as follows:

[0058] Connection between the robotic arm and the trolley: A six-degree-of-freedom robotic arm (with repeatability accuracy ≤ ±0.05mm) is selected and fixed to the top base of the Ackerman trolley through an integrated aluminum alloy flange. The four corners of the flange are symmetrically locked with four sets of M8 anti-loosening bolts (with spring washers) to ensure that there is no relative displacement between the two.

[0059] Sensor Installation: A depth camera is fixed 50mm in front of the end flange of the robotic arm using an L-shaped aluminum alloy bracket; an IMU is installed at the center of the trolley chassis, and the sensor is connected via I... 2 The C-bus communicates with the vehicle's main control board (such as an NVIDIA Jetson series or equivalent processor);

[0060] Data synchronization: The camera, IMU, and robotic arm controller are connected to the Noetic version of the ROS system, and data is synchronized through ros::Time timestamps to ensure that the interaction latency between the three is <10ms (the measured average is 8.2ms).

[0061] S2. Dual coordinate system precise calibration: including robot arm-camera extrinsic parameter calibration and trolley-robot arm coordinate system calibration;

[0062] S2.1, such as Figure 2 As shown, the robotic arm-camera extrinsic parameter calibration specifically includes:

[0063] S2.1.1 Environment Configuration: Start the robot arm driver node robot_arm_driver, the depth camera acquisition node camera_publisher, and the easy_handeye calibration node easy_handeyecalibrate. Configure the calibration parameters in the ROS parameter server (including the camera coordinate system name camera_link, the robot arm end coordinate system name end_effector_link, calibration plate size parameters, etc.).

[0064] S2.1.2 Calibration board deployment: Fix the checkerboard calibration board to a stationary plane (such as a wall or workbench), ensuring that the plane of the calibration board is approximately perpendicular to the center of the camera's field of view, and is within the preset working distance range of the camera (such as D±0.2D, where D is the typical grasping distance).

[0065] S2.1.3, Pose Sample Acquisition: The robot arm is driven by a robotic arm teaching pendant or program-controlled robotic arm to move the camera. Within the pose space covering pitch angle -α to +α, rotation angle 0 to 360°, and translation range ±β, N sets of valid samples are collected (N≥15, each set of samples must meet the calibration board corner point recognition rate ≥80%, and the pose difference between adjacent samples is significant).

[0066] S2.1.4, Extrinsic Parameter Calculation and Storage: The easy_handeye calibration package automatically calculates the extrinsic parameter matrices of the camera coordinate system and the robot arm end effector coordinate system based on the acquired samples (rotation matrix error < 0.1°, translation vector error < 0.3mm), and saves the extrinsic parameter file handeye_calibration.yaml to the config directory of the ROS workspace for subsequent coordinate transformation.

[0067] S2.2, such as Figure 3 As shown, the calibration of the trolley-robotic arm coordinate system is specifically as follows:

[0068] S2.2.1 Trajectory Deployment: Draw a straight trajectory of length L on a flat surface, and uniformly mark n sampling points (n≥5, including the starting point A (X=0, Y=0), the ending point B (X=L, Y=0), and intermediate points) along the trajectory. Use high-precision measuring tools (such as laser trackers or digital calipers with an accuracy of ≤0.02mm) to calibrate the coordinates of each point and the straightness of the trajectory, ensuring that the deviation between adjacent points is <0.1mm;

[0069] S2.2.2 Multi-position pose acquisition, including:

[0070] ① Park the trolley at starting point A, and read the coordinates of the origin 8 of the robotic arm base coordinate system in the trolley base coordinate system (origin 9 of the trolley base coordinate system, such as the center of the rear wheel axle) through the robotic arm controller: ;

[0071] ② Control the trolley to travel along the trajectory at a constant speed v, stopping sequentially at the midpoints. At endpoint B, after each stop, keep the trolley stationary for 3 seconds (to eliminate the influence of motion inertia), and read the coordinates of the origin 8 of the robotic arm base coordinate system: ;

[0072] S2.2.3 Outlier Removal: For the collected n sets of coordinate data... (i=1,2,…,n) A consistency test is performed, and the 3σ criterion is used to remove outliers that deviate from the mean by more than 3 times the standard deviation (if the number of samples after removal is <3, samples are collected again).

[0073] S2.2.4, Transformation relation fitting, including:

[0074] ① Based on valid sample data, establish a linear relationship model between the origin coordinates of the robotic arm base coordinate system and the coordinates of the trolley base coordinate system: , (Under ideal rigid connection) (Slight deviations may exist due to installation errors).

[0075] ② Use the least squares method to analyze the model parameters. To perform fitting, the fitting residual should be ≤0.1mm (i.e., the deviation between the actual coordinates and the fitted values ​​should be <0.1mm).

[0076] ③ Verify the consistency of rigid connections: If or This indicates a non-rigid connection between the robotic arm and the cart (e.g., looseness), requiring re-tightening and recalibration; otherwise, take k. x =1, kᵧ=1, determine the fixed conversion formula: , (ΔX and ΔY are constant offsets obtained from fitting), and the formula is solidified into the positioning compensation algorithm.

[0077] S3. IMU Integration and Calibration: Inertial Measurement Unit (IMU) is integrated on the vehicle. The IMU coordinate system is aligned and calibrated with the vehicle's motion coordinate system so that the IMU's X-axis is consistent with the vehicle's forward direction and the Z-axis is consistent with the direction of gravity. The IMU sampling frequency is not less than 100Hz.

[0078] Furthermore, the IMU employs a Kalman filter algorithm to calculate pose changes. By dynamically adjusting the process noise covariance matrix Q and the observation noise covariance matrix R, the pose calculation accuracy is improved in scenarios with uneven ground and vehicle acceleration. The angular acceleration measurement error is <0.01 rad / s², the linear acceleration measurement error is <0.02 m / s², and the IMU sampling rate is 100-200 Hz.

[0079] S4, Target Detection and Compensation Trigger:

[0080] Before target detection, there is also an accuracy phase, as detailed below:

[0081] Target object deployment: Fix a sample rack next to the trajectory and place a target object 10 (such as an ABS sample tube with a diameter of d and a height of h) on it. The visual positioning mark is attached to the surface of the sample tube and the size of the mark is adapted to the proportion of the target to improve recognition stability.

[0082] Truth value acquisition: Use high-precision measuring equipment (such as a laser tracker or digital caliper, with a measurement accuracy ≤ 0.01 mm) to acquire the absolute coordinates of the target object's center in the ground coordinate system. , serving as the true benchmark for comparing positioning accuracy;

[0083] Parameter presets: Based on the depth camera performance (resolution, depth accuracy) and the robotic arm's workspace, the preset "optimal gripping range" is: (in To ensure a safe proximity threshold, collisions between the robotic arm and the target / sample holder should be avoided; This is a precision threshold; exceeding this threshold will significantly increase the visual positioning error. Example range: ).

[0084] Furthermore, such as Figure 4 As shown, the specific content of target detection and compensation triggering in step S4 is as follows:

[0085] S41. Car Motion Control: Control the moving car to travel at a constant speed from the starting point A (X_A, Y_A) along a preset path towards the area where the target object is located. The speed is v (0.1m / s≤v≤0.3m / s, matching the typical moving speed of the car in the actual scene).

[0086] S42. Real-time target perception: The depth camera at the end of the robotic arm continuously captures images of the target object at a frame rate f_cam (f_cam≥30fps). The target is identified through visual algorithms (such as deep learning-based target detection or traditional feature point matching), and the real-time distance D between the target and the camera is calculated based on the camera depth map.

[0087] S43. Compensation trigger condition: When a compensation trigger condition is detected... When the target enters the optimal capture range, the dynamic compensation process is triggered, and this moment is recorded as the "compensation trigger moment". ".

[0088] Furthermore, the compensation process for this step is as follows:

[0089] The target is identified by a deep learning-based target detection algorithm, and the center pixel coordinates (u, v) of the target are extracted. Combined with the camera intrinsic parameter matrix K, the pixel coordinates are converted into three-dimensional coordinates in the camera coordinate system using formula (1). ),

[0090] (1)

[0091] Among them, the camera intrinsic parameter matrix , For camera focal length, Z_cam represents the coordinates of the primary point, obtained from the camera's factory calibration; Z_cam represents the depth value of the target point in the camera's depth map.

[0092] Calculate the real-time distance between the target and the camera. When D is within the preset optimal capture range, a dynamic compensation process is triggered.

[0093] S5. Initial pose acquisition, such as Figure 4 Specifically, it is shown as follows:

[0094] S51, in At any given moment, following the coordinate transformation process, based on the camera intrinsic parameters, the "robotic arm-camera" extrinsic parameter matrix (R,T), and the forward kinematics of the robotic arm, the transformation of "image coordinates → camera coordinates → robotic arm end-effector coordinates → robotic arm base coordinates" is completed sequentially.

[0095] S52, The "Cart-Robot Arm Coordinate System Transformation Relationship" obtained from calibration ( , The above-mentioned robot arm base coordinates are used to calculate the initial pose of the target in the vehicle coordinate system. ;

[0096] S53. Accuracy Verification: Calculate the initial pose and the true value. The deviation, the required error This ensures that the location of the trigger point is reliable.

[0097] Furthermore, obtaining the initial pose in this step specifically includes:

[0098] At the time of compensation triggering By combining the external parameter matrices (R, T) of the camera and the end of the robotic arm, the coordinates (X_cam, Y_cam, Z_cam) of the target in the camera coordinate system are converted into the coordinates (X_end, Y_end, Z_end) in the end of the robotic arm coordinate system using formula (2).

[0099] (2)

[0100] Then, by using the forward kinematics formula (3) of the robotic arm, combined with The angles of each joint of the robotic arm at all times Calculate the coordinates in the robot arm base coordinate system. ;

[0101] (3)

[0102] Where f(x) is the forward kinematics function of the robotic arm, which is derived based on the DH parameter method. The DH parameters are provided by the robotic arm manufacturer or calibrated by actual measurement.

[0103] Finally, based on the fixed conversion relationship between the trolley and the robotic arm ( , The initial pose of the target in the car coordinate system is obtained by inverse calculation. .

[0104] S6. Dynamic offset perception and pose correction, specifically:

[0105] S61, IMU dynamic offset perception: the car is in After a certain time, the vehicle continues to travel, and the onboard IMU collects acceleration and angular velocity data in real time at a sampling frequency of no less than 100Hz, and calculates the vehicle's relative acceleration and angular velocity according to the described Kalman filter algorithm. The pose changes at time (ΔX, ΔY, Δθ), where the rotation angle Δθ is used to correct the displacement offset in the X and Y directions through coordinate rotation transformation.

[0106] S62. Real-time target pose correction:

[0107] ① The real-time pose of the target in the car coordinate system is calculated according to formula (4), that is, the pose change (ΔX, ΔY, Δθ) obtained by IMU is used to dynamically correct the initial pose;

[0108]

[0109] (4)

[0110] ;

[0111] When Δθ < 0.5°, the rotation correction term can be ignored, and formula (4) can be simplified to , (Note: If Δθ < 0.5°, the rotation correction term can be ignored, simplifying to...) , ).

[0112] ② Convert to robot arm base coordinate system pose: Combine the fixed offset (ΔX_offset, ΔY_offset) between the "cart and robot arm" to obtain the corrected pose that the robot arm end effector needs to track: ; ; (The target's height in the Z direction remains unchanged).

[0113] S7. Dynamic capture and execution, specifically:

[0114] Robotic arm dynamic tracking: The robotic arm controller calculates the target angle of each joint through inverse kinematics based on the corrected pose (X_arm, Y_arm, Z_arm), and sends motion commands (such as ROSJointTrajectory or URScript messages) with a command update cycle τ (i.e., the servo cycle of the robotic arm controller, τ≤10ms) to ensure that the end effector tracks the target position in real time; the delay t_delay of a single compensation cycle (from IMU data acquisition to robotic arm command execution) is <10ms. In this preferred embodiment, after 30 repeated tests, the average delay is 9.2ms, which meets the requirement of ≤10ms.

[0115] Grasping execution and confirmation: When the end effector of the robotic arm approaches the target, the contact force F is detected in real time by the end force sensor. When F≥F' (F' is the contact threshold, set according to the target weight, example: 1N≤F'≤5N), the gripper closing command is triggered, and the target is grasped while the trolley continues to move.

[0116] Specifically, in this step, the inverse kinematics formula (5) of the robotic arm is used to calculate the target angles of each joint. , ,..., ), generate motion commands for the robotic arm's end effector;

[0117] ; (5)

[0118] Among them, f -1 (x) is the inverse kinematics function of the robotic arm, which is solved by numerical iteration to ensure the uniqueness and real-time performance of the solution.

[0119] To further verify the effectiveness of the present invention, corresponding comparative tests were conducted:

[0120] (1) Experimental equipment:

[0121] Core hardware: The six-DOF robotic arm uses a Jakamin2; the depth camera is an Intel RealSense D435i; the IMU is an MPU6050; the mobile vehicle is an Ackermann ROS intelligent vehicle; the main control board is an NVIDIA Orin Nano 4GB. Target object: An ABS sample tube, fixed 300mm lateral to the track. The absolute coordinates of its center in the ground coordinate system are measured using a digital caliper (accuracy ±0.02mm). .

[0122] Experimental scenario: Indoor ground, using LED constant illumination light source (to avoid natural light fluctuations affecting camera imaging), the car's driving trajectory is 1.5m long, and the preset depth camera "optimal grasping range" is 0.5-1.5m.

[0123] (2) Test plan:

[0124] Experimental group (dual calibration + IMU compensation):

[0125] ① Robotic arm-camera extrinsic parameter calibration: 20 sets of samples were collected using the easy_handeye package, and the rotation matrix error was found to be 0.09° and the translation vector error was 0.28mm;

[0126] ② Cart-robotic arm calibration: Six sampling points were collected along a 1.5m trajectory. The fixed offset ΔX=151mm and ΔY=0.1mm were obtained by least squares fitting, and the fitting residual was 0.07mm.

[0127] ③ Compensation strategy: The IMU data uses a complementary filtering algorithm to calculate the pose change, the robotic arm command update cycle is 10ms, and the average compensation cycle delay is 9.2ms.

[0128] Control group (no calibration + no compensation): Relying only on the initial visual positioning of the camera (without coordinate transformation), the robot arm does not update its instructions during the movement of the trolley, and only performs grasping according to the initial pose at the trigger time.

[0129] (3) Results and Analysis: The experiment was repeated 30 times. The positioning deviation of the robotic arm end was recorded using a laser tracker (accuracy ±0.01mm). The criterion for judging the success rate of grasping was "the target did not fall off after the gripper closed and the center offset was ≤2mm".

[0130] Experimental group: The average positioning deviation of the end effector was 0.75mm (maximum deviation 1.2mm, minimum 0.3mm), and the dynamic grasping was successful 28 times, with a success rate of 93.3%.

[0131] Control group: The average end-effector positioning deviation was 5.32 mm (affected by the cumulative effect of trolley movement offset and coordinate transformation error), with 17 successful grasps and a success rate of 56.7%. The significant difference between the two groups verifies that the synergistic effect of "dual calibration to eliminate coordinate transformation error + IMU real-time compensation for movement offset" can effectively improve the accuracy and reliability of dynamic grasping by the mobile robotic arm.

[0132] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The program can be stored in a non-volatile computer-readable storage medium, and when executed, it can include the processes of the embodiments of the above methods. Any references to memory, storage, databases, or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), RAMbus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and RAMbus dynamic RAM (RDRAM), etc.

[0133] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0134] The above embodiments are preferred embodiments of the present invention, but the embodiments of the present invention are not limited to the above embodiments. Any changes, modifications, substitutions, combinations, or simplifications made without departing from the spirit and principle of the present invention shall be considered equivalent substitutions and shall be included within the protection scope of the present invention.

Claims

1. A dynamic grasping and positioning compensation method for a mobile robotic arm based on an IMU and hand-eye system, characterized in that, Includes the following steps: S1. Rigid connection and vision system construction: The six-degree-of-freedom robotic arm is fixed to the base on the top of the mobile trolley through a rigid flange. A depth camera facing the target object is rigidly installed at the end of the robotic arm to form a hand-eye system. S2. Dual-coordinate system precise calibration: A checkerboard calibration board is used to control the robotic arm to drive the camera to take multiple sets of images in various postures. The external parameter matrix of the camera coordinate system and the robotic arm end coordinate system is solved by the hand-eye calibration algorithm. The trolley is controlled to move at a constant speed along a preset straight trajectory on the ground, and the pose data of the trolley base coordinate system and the robotic arm coordinate system are collected to establish a fixed transformation relationship between the two. S3. IMU Integration and Calibration: Inertial Measurement Unit (IMU) is integrated on the vehicle. The IMU coordinate system is aligned and calibrated with the vehicle's motion coordinate system so that the IMU's X-axis is consistent with the vehicle's forward direction and the Z-axis is consistent with the direction of gravity. The IMU sampling frequency is not less than 100Hz. S4. Target Detection and Compensation Trigger: Control the car to move towards the target object. The depth camera continuously captures images of the target object at a set frame rate. The target center pixel coordinates are extracted using a target detection algorithm. Combined with the camera intrinsic parameter matrix, the pixel coordinates are converted into three-dimensional coordinates in the camera coordinate system, and the real-time distance between the target and the camera is calculated. When the distance is within the optimal grasping range, the dynamic positioning compensation process is triggered, and the compensation trigger moment is recorded. ; S5. Initial pose acquisition: In At any given moment, the coordinates of the target in the camera coordinate system are converted to the coordinates in the robot arm end-effector coordinate system by combining the extrinsic parameter matrices of the camera and the robot arm end-effector coordinate system. Then, the coordinates are converted to the coordinates in the robot arm base coordinate system by using the forward kinematics formula of the robot arm. Finally, the initial pose of the target in the car coordinate system is calculated by back-calculating based on the fixed transformation relationship between the car and the robot arm. S6. Dynamic Offset Sensing and Pose Correction: As the vehicle continues to move, the IMU collects angular acceleration and linear acceleration data in real time, and calculates the vehicle's position relative to the ground using a Kalman filter algorithm. The pose changes ΔX, ΔY, and Δθ at each moment are combined with the fixed transformation relationship between the trolley and the robotic arm to superimpose the pose changes onto the initial pose of the target, thus obtaining the corrected pose of the target in the current coordinate system of the robotic arm base. S7. Dynamic Grasping Execution: The target angles of each joint are calculated based on the corrected pose using the inverse kinematics formula of the robotic arm. The robotic arm controller sends joint target angle commands with an instruction update cycle of ≤10ms, driving the robotic arm to adjust in real time. When the end effector of the robotic arm detects a contact force ≥1~5N with the target object, it triggers the gripper closing command, completing the target grasping during the continuous movement of the trolley, and the single compensation cycle delay is ≤10ms.

2. The dynamic grasping and positioning compensation method for a mobile robotic arm based on an IMU and hand-eye system according to claim 1, characterized in that, In step S1, the rigid flange is an integrally formed aluminum alloy flange, and the relative displacement deformation between the rigid flange and the base is within 0.1 mm.

3. The method for dynamic grasping and positioning compensation of a mobile robotic arm based on an IMU and hand-eye system according to claim 1, characterized in that, In step S2, the robotic arm drives the camera to capture images in multiple poses, covering a pitch angle of -30° to +30° and a rotation angle of 0 to 360°, and the calibration board corner point recognition rate of each set of images is ≥80%.

4. The dynamic grasping and positioning compensation method for a mobile robotic arm based on an IMU and hand-eye system according to claim 1, characterized in that, The IMU uses the Kalman filter algorithm to calculate the pose change. By dynamically adjusting the process noise covariance matrix Q and the observation noise covariance matrix R, the pose calculation accuracy is improved in scenarios with uneven ground and vehicle acceleration. The angular acceleration measurement error is <0.01 rad / s², the linear acceleration measurement error is <0.02 m / s², and the IMU sampling rate is 100-200 Hz.

5. The dynamic grasping and positioning compensation method for a mobile robotic arm based on an IMU and hand-eye system according to claim 1, characterized in that, In step S4, the target center pixel coordinates are extracted using a target detection algorithm, and then converted into three-dimensional coordinates in the camera coordinate system using the camera intrinsic parameter matrix. The real-time distance between the target and the camera is then calculated. Specifically: The target is identified by a deep learning-based target detection algorithm, and the center pixel coordinates (u, v) of the target are extracted. Combined with the camera intrinsic parameter matrix K, the pixel coordinates are converted into three-dimensional coordinates in the camera coordinate system using formula (1). ), （1） Among them, the camera intrinsic parameter matrix , For camera focal length, Z_cam represents the coordinates of the primary point, obtained from the camera's factory calibration; Z_cam represents the depth value of the target point in the camera's depth map. Calculate the real-time distance between the target and the camera. When D is within the preset optimal capture range, a dynamic compensation process is triggered.

6. The method for dynamic grasping and positioning compensation of a mobile robotic arm based on an IMU and hand-eye system according to claim 1, characterized in that, Step S5 is as follows: At the time of compensation triggering Combining the extrinsic parameter matrices (R, T) of the camera and the end effector of the robotic arm, the coordinates of the target in the camera coordinate system are obtained using formula (2). Convert the coordinates to the coordinates in the robotic arm's end effector coordinate system. ); （2） Then, by using the forward kinematics formula (3) of the robotic arm, combined with The angles of each joint of the robotic arm at all times Calculate the coordinates in the robot arm base coordinate system. ); （3） Where f(x) is the forward kinematics function of the robotic arm, which is derived based on the DH parameter method. The DH parameters are provided by the robotic arm manufacturer or calibrated by actual measurement. Finally, based on the fixed conversion relationship between the trolley and the robotic arm: , The initial pose of the target in the car coordinate system is obtained by inverse calculation. .

7. The method for dynamic grasping and positioning compensation of a mobile robotic arm based on an IMU and hand-eye system according to claim 1, characterized in that, In step S6, the filtering process is as follows: Equations of state: Wherein, the state vector x and y represent translational displacements, and θ represents the rotation angle. ω is the linear velocity, ω_z is the angular velocity; A is the state transition matrix, B is the input matrix. The acceleration input is collected by the IMU, and w_k is the process noise; Observation equation: Wherein, the observation vector Z_k is composed of the IMU measured velocity and angular velocity, H is the observation matrix, and v_k is the observation noise; Filtering update: Through iterative prediction and update steps, the optimal pose change is output. , , , for The car's position at all times.

8. The method for dynamic grasping and positioning compensation of a mobile robotic arm based on an IMU and hand-eye system according to claim 1, characterized in that, In step S6, the pose change is superimposed on the initial pose of the target, based on the fixed transformation relationship between the vehicle and the robotic arm, to obtain the corrected pose of the target in the current coordinate system of the robotic arm base. Specifically: Combining the transformation relationship of the "cart-robotic arm" calibration, the pose change is superimposed on the initial pose of the target using formula (4) to obtain the corrected pose of the target in the current coordinate system of the robotic arm base. ); （4） ； Then, using the inverse kinematics formula (5) for the robotic arm, the target angles of each joint are calculated. ), generate motion commands for the robotic arm's end effector; ； （5） Among them, f -1 (x) is the inverse kinematics function of the robotic arm, which is solved by numerical iteration to ensure the uniqueness and real-time performance of the solution.

Images