Mobile manipulator working method and system based on visual guidance and dynamic compensation

Abstract

The application relates to a mobile mechanical arm operation method and system based on visual guidance and dynamic compensation, and relates to the technical field of intelligent robots; the method is characterized in that a mobile chassis is autonomously navigated and parked, target pose is obtained by combining visual key point detection and six-degree-of-freedom pose solving, and after coordinate system conversion, two-way dynamic compensation of mechanical arm trajectory correction and chassis barycenter stability is synchronously realized, and finally the mechanical arm is controlled to complete the operation. The application improves the positioning accuracy through a two-stage visual processing mode, reduces the influence of parking deviation and barycenter change on end positioning through two-way dynamic compensation of the chassis and the mechanical arm, and guarantees man-machine safety through a three-stage safety protection system, and can be widely applied to unmanned operation scenes such as warehouse sorting, workshop material transfer and automatic assembly.

Description

Technical Field

[0001] This invention relates to the fields of automated robots, machine vision and intelligent control technology, and in particular to a mobile robotic arm operation method and system based on vision guidance and dynamic compensation, which can be applied to unmanned and intelligent operation scenarios such as automated inspection, warehouse sorting, workshop material transfer, assembly, testing and laboratory sampling. Background Technology

[0002] With the rapid development of intelligent manufacturing and smart logistics, single-function mobile robots or fixed robotic arms can no longer meet the needs of complex work scenarios. Mobile robots (AGVs / AMRs) have autonomous navigation capabilities but lack flexible operation capabilities; fixed robotic arms have precise grasping and handling capabilities, but their operating range is limited by their fixed base. Therefore, integrating mobile chassis with robotic arms to form mobile composite robots has become an industry trend.

[0003] Several composite robot solutions combining AGVs and robotic arms exist in the current technology. For example, Chinese patent application CN117381780A discloses a dynamic grasping method and system for composite robots based on visual servoing. This method acquires a depth image of the composite robot in its travel direction in real time, and obtains the coordinate information of the object to be grasped and the current pose of the composite robot based on the depth information of the depth image and the position of the object to be grasped in the image, thus solving the problem of motion incoordination caused by the different response speeds of the mobile platform and the robotic arm. Chinese patent application CN119973954A discloses a CNC loading and unloading composite robot system and its accuracy compensation method. This method calculates the deformation of the chassis caused by the movement of the robotic arm and compensates for errors by measuring the stiffness of the AGV chassis and the superstructure platform. Chinese patent application CN119704183A discloses a mobile robotic arm grasping system based on a reinforcement learning collaborative controller. This system uses a reinforcement learning collaborative controller to synchronously generate control commands for the mobile chassis and the robotic arm according to the target position.

[0004] In general, existing mobile robot and robotic arm hybrid systems suffer from the following technical shortcomings:

[0005] 1. Relying on a single type of sensor, it is prone to perception blind spots in complex industrial scenarios such as transparent objects, low obstacles, and changes in lighting, and lacks a multi-level, multi-modal redundant perception mechanism.

[0006] 2. Most of them adopt serial control mode, and there is a lack of closed-loop interaction between them based on real-time status. Chassis docking deviation and changes in the center of gravity caused by the load of the robotic arm will directly cause end positioning error.

[0007] 3. The security protection system is incomplete, mainly relying on software-level obstacle avoidance, lacking physical safety redundancy at the hardware level, and there is no effective backup guarantee when sensors fail.

[0008] 4. Data is collected independently by various sensors, lacking a unified spatiotemporal reference, which affects the accuracy of perception and the real-time performance of control in dynamic environments. Summary of the Invention

[0009] The technical problem to be solved by the present invention is to provide a mobile robotic arm operation method and system based on vision guidance and dynamic compensation, so as to solve the technical problems of insufficient perception redundancy and safety protection, limited visual positioning accuracy, lack of real-time closed-loop interaction between chassis and robotic arm collaborative control, and lack of multi-sensor spatiotemporal synchronization and fusion mechanism in existing mobile robotic arm composite systems.

[0010] The technical solution adopted by this invention to solve its technical problem is: a mobile robotic arm operation method based on vision guidance and dynamic compensation, comprising the following steps:

[0011] The mobile chassis autonomously plans its route according to mission instructions and drives to the target location for parking.

[0012] The vision module acquires images of the work area, performs coarse detection of key points of the target object through a deep learning network, and constructs a local region of interest based on the enclosing region formed by the coarsely detected key points. Within the local region of interest, sub-pixel level corner point refinement is performed using traditional vision methods to obtain the corrected corner point coordinates. Then, based on the correspondence between the corrected corner point coordinates and the points of the 3D model of the target object, the six-degree-of-freedom pose of the target object is solved.

[0013] Based on the camera mounting method, the pose of the target object in the camera coordinate system is transformed to the robot base coordinate system;

[0014] During the operation of the robotic arm, the control system acquires the real-time posture of the chassis, the current joint status of the robotic arm and the end-effector load information, and generates compensation based on the chassis posture deviation and the robotic arm status to correct the movement trajectory of the robotic arm in real time.

[0015] When the deep learning network fails to perform coarse detection, the coarse detection result is used as the output directly; when the subpixel refinement fails, it automatically reverts to the deep learning coarse detection result.

[0016] Furthermore, the vision module of this invention acquires images of the work area, and after image acquisition, it uses Letterbox to perform proportional scaling and black border filling, with a scaling ratio of... Where W is the width of the original input image, H is the height of the original input image, Wt is the width of the network input size, Ht is the height of the network input size, and the horizontal offset is recorded. and vertical offset After network output, through =( - ) / s、 =( - ) / s maps keypoints back to their original image coordinates, where , To recover the key point coordinates from the original image, , These are the coordinates of key points output by the deep learning network.

[0017] Furthermore, the deep learning network of this invention employs a lightweight keypoint detection network, which outputs the target category, confidence level, and at least four two-dimensional keypoints. Post-processing filters candidate results with confidence levels below 0.35 and uses non-maximum suppression to remove duplicate detections, with the non-maximum suppression threshold set to 0.45.

[0018] Furthermore, this invention expands the bounding region formed by the coarse detection of key points in deep learning outward by 35% to form a local region of interest; the local region of interest is then subjected to grayscale conversion, Gaussian filtering, Canny edge detection, morphological dilation, main contour screening, and geometric fitting in sequence, and the corrected corner points are obtained by approximation with the minimum bounding rectangle or polygon, and are uniformly sorted in the order of top left, top right, bottom right, and bottom left.

[0019] Furthermore, this invention uses the RANSAC-PnP algorithm to solve the six-degree-of-freedom pose of the target object, sets the reprojection error threshold to 8 pixels, and sets the maximum number of iterations to 100. After solving, the reprojection error is calculated. When the reprojection error is less than 1 pixel, it is determined to be highly reliable. When it is between 1 and 3 pixels, it is determined to be usable. When it is greater than 5 pixels, it is determined to be unreliable. Unreliable poses are not sent to the robotic arm controller.

[0020] Furthermore, for the camera mounting method where the eye is outside the hand, the pose of the target object in the robot's base coordinate system satisfies the following conditions: For the camera mounting method with the eye on the hand, the pose of the target object in the robot's base coordinate system satisfies Where T represents the homogeneous transformation matrix, obj represents the target object coordinate system, cam represents the camera coordinate system, end represents the end effector coordinate system, and robot represents the robot base coordinate system.

[0021] Furthermore, the control system of the present invention estimates the projected position of the center of gravity of the composite system based on the joint angle of the robotic arm, the end load, and the extension direction. When the projected center of gravity approaches the preset stable boundary, the speed and acceleration of the robotic arm joint are reduced, or the chassis is driven to make a millimeter-level micro-displacement in the direction opposite to the center of gravity offset, so that the projected center of gravity returns to the stable region.

[0022] Furthermore, the present invention also includes safety protection steps: when the non-contact sensor detects an obstacle, the robot is controlled to slow down or detour; when the non-contact sensor fails to detect an obstacle and a collision occurs, the contact safety edge is squeezed and outputs a trigger signal to immediately trigger the shutdown protection; when the operator presses the emergency stop switch, the hardware circuit directly cuts off the power; the non-contact sensor includes a lidar, a depth camera, and an ultrasonic sensor.

[0023] Furthermore, the control system of this invention uses a unified control cycle to synchronize the chassis positioning data, the robotic arm joint status and the visual pose in time, with the visual update cycle not exceeding 40ms; when the time difference between the chassis status and the visual pose exceeds the allowable threshold or the visual pose is determined to be unreliable, the execution of a new grasping trajectory is paused and the data is resynchronized.

[0024] Meanwhile, the present invention also provides a mobile robotic arm operation system based on vision guidance and dynamic compensation, comprising:

[0025] The mobile chassis module executes the chassis autonomous navigation steps described above.

[0026] The robotic arm operation module is installed on the mobile chassis. It is a six-degree-of-freedom robotic arm with a gripper or suction cup at the end to perform the operation actions in the above method.

[0027] The multimodal perception module, including LiDAR, depth camera, ultrasonic sensor and contact safety edge, executes the visual guidance steps and safety protection steps in the above method;

[0028] The control system module is communicatively connected to the mobile chassis module, the robotic arm operation module, and the multimodal perception module, respectively, and executes the coordinate transformation step and the dynamic compensation step in the above method.

[0029] The beneficial effect of this invention is that it solves the defects existing in the prior art.

[0030] 1. It breaks the traditional serial control paradigm of chassis stabilization before operation. Through bidirectional dynamic compensation between the chassis and the robotic arm, it cancels out the error effects of chassis docking deviation, ground disturbance, and load center of gravity change in a closed loop, which greatly improves the positioning stability and accuracy of the end effector.

[0031] 2. A two-level localization strategy is adopted, which combines deep learning coarse detection with traditional visual sub-pixel refinement. With the addition of a letterbox coordinate inversion mechanism and reprojection error reliability verification, it not only ensures adaptability in complex industrial scenarios, but also achieves sub-pixel level localization accuracy. Moreover, the visual end-to-end latency can be controlled within 40ms, meeting the requirements for real-time feedback.

[0032] 3. A three-level redundancy system has been constructed, from non-contact early warning, contact physical protection to hardware emergency stop. Even in extreme scenarios where all sensing sensors fail, a safe shutdown can still be triggered by physical contact, effectively ensuring human and machine safety.

[0033] 4. Multimodal sensor fusion perception makes up for the blind spots of a single sensor. With the help of a unified spatiotemporal synchronization mechanism, the system can still maintain stable perception and operation capabilities in industrial scenarios such as changes in lighting, transparent obstacles, and complex backgrounds.

[0034] 5. Based on the ROS2 layered modular architecture, it supports secondary development and function expansion; it also supports three operation modes: manual, semi-automatic and fully automatic, which can adapt to the operation needs of different scenarios. Attached Figure Description

[0035] Figure 1 This is a schematic diagram of the system structure of the present invention;

[0036] Figure 2 This is a block diagram of the hierarchical architecture of the system of this invention;

[0037] Figure 3 This is a flowchart of the collaborative control process between the chassis and the robotic arm of this invention;

[0038] Figure 4 This is a diagram of the three-level security protection system architecture of this invention;

[0039] In the diagram: 1. Mobile chassis module; 2. Robotic arm operation module. Detailed Implementation

[0040] The present invention will now be described in further detail with reference to the accompanying drawings and preferred embodiments. These drawings are simplified schematic diagrams, illustrating only the basic structure of the invention, and therefore only show the components relevant to the invention.

[0041] like Figures 1-4 The illustrated method and system for operating a mobile robotic arm based on vision guidance and dynamic compensation.

[0042] The overall system architecture includes:

[0043] Mobile chassis module 1: It adopts a differential drive chassis and has autonomous navigation, SLAM mapping, global path planning and dynamic obstacle avoidance functions;

[0044] Robotic arm operation module 2: Installed on the mobile chassis, it is a six-degree-of-freedom robotic arm with an end effector such as a gripper or suction cup, responsible for grasping, transporting, and placing operations.

[0045] Multimodal perception module: including LiDAR, depth camera, ultrasonic sensor and contact safety edge, distributed and installed in various parts of the robot body to achieve all-round perception of the environment;

[0046] Control system module: including the main control unit and the layered software architecture running on it, realizing functions such as navigation and positioning, motion control, visual recognition, and collaborative operation scheduling;

[0047] Power supply and safety module: including lithium battery pack, battery management system (BMS) and emergency stop switch, providing power and safety protection.

[0048] A key feature of this invention is the construction of a multi-layered, multi-modal perception-based security protection system, specifically including:

[0049] The first layer, the non-contact environmental perception layer, includes:

[0050] LiDAR: Installed inside the AMR chassis, used for 2D environmental contour scanning, SLAM mapping, and long-range obstacle detection;

[0051] Depth camera (RGB-D): Mounted at the front of the camera body, used to acquire color images and depth information to achieve target recognition, object segmentation and grasping coordinate calculation;

[0052] Ultrasonic sensors: Arranged in a ring around the body, they are used to detect transparent objects, glass, and low obstacles, compensating for the blind spots of LiDAR and depth cameras in these scenarios.

[0053] The second layer, the contact-based physical safety layer, includes:

[0054] Contact-type safety edge (anti-collision strip): Installed on the bottom outermost ring around the robot chassis. When the robot collides with an object, the anti-collision strip is squeezed and outputs a switch signal, immediately triggering a shutdown protection, serving as the last physical safety redundancy after the failure of non-contact sensors.

[0055] The third layer, the hardware emergency stop layer, includes:

[0056] Emergency stop switch (mushroom head button): Installed in a conspicuous and easily accessible position on the side and top of the robot body, it uses normally closed contacts to connect to a safety hard circuit. In an emergency, it directly cuts off the motor power supply, which cannot be bypassed by the software. It is the highest priority safety protection.

[0057] This invention employs a three-level collaborative control logic: chassis-first positioning, vision-precise guidance, and robotic arm dynamic compensation.

[0058] Level 1, Autonomous Navigation and Precise Docking of Chassis: The mobile chassis autonomously plans a global path based on the SLAM map according to the mission instructions, and achieves precise docking after driving to the target point (positioning accuracy ±10mm).

[0059] Level 2, Visual Guidance and Coordinate Calculation: After the chassis completes its initial docking, the vision module images the work area and obtains the position and orientation of the target object in the robot's base coordinate system by following the process of image acquisition and proportional preprocessing, deep learning key point coarse detection, traditional visual sub-pixel refinement, RANSAC-PnP six-DOF pose solving, credibility assessment, and conversion from the camera coordinate system to the robot's base coordinate system. This serves as a unified spatial reference for the robotic arm's motion planning and subsequent dynamic compensation.

[0060] The specific method includes the following steps:

[0061] The chassis autonomous navigation process involves the mobile chassis autonomously planning its path according to mission instructions and driving to the target location for parking.

[0062] The visual guidance step involves the visual module acquiring images of the work area, using a deep learning network to perform coarse detection on key points of the target object, and constructing a local region of interest based on the enclosing region formed by the coarsely detected key points. Within the local region of interest, sub-pixel level corner point refinement is performed using traditional visual methods to obtain the corrected corner point coordinates. Then, based on the correspondence between the corrected corner point coordinates and the points of the 3D model of the target object, the six-degree-of-freedom pose of the target object is solved.

[0063] The coordinate transformation step involves transforming the pose of the target object in the camera coordinate system to the robot base coordinate system according to the camera mounting method.

[0064] The dynamic compensation step involves the control system acquiring the real-time pose of the chassis, the current joint status of the robotic arm, and the end-effector load information during the operation of the robotic arm. Based on the chassis pose deviation and the robotic arm status, a compensation amount is generated to correct the movement trajectory of the robotic arm in real time.

[0065] In the visual guidance step, when the deep learning network fails to perform coarse detection, the coarse detection result is directly used as the output; when the sub-pixel refinement fails, it automatically reverts to the deep learning coarse detection result.

[0066] The specific implementation method of the visual guidance steps is as follows:

[0067] (1) Image acquisition and preprocessing. An industrial camera or RGB-D camera acquires images of the target area, preferably using Letterbox for proportional scaling and black border filling: scaling ratio Where W is the width of the original input image, H is the height of the original input image, Wt is the width of the network input size, Ht is the height of the network input size, and s is the image scaling ratio, and the horizontal and vertical offsets are recorded. , ,in The horizontal coordinate offset generated when filling the Letterbox The vertical coordinate offset generated by filling the Letterbox is then passed through the network output. =( - ) / s、 =( - The ) / s method accurately maps key points back to the original image coordinates, avoiding systematic coordinate deviations caused by non-proportional stretching from the source. To restore the horizontal coordinates of key points in the original image, To restore the vertical coordinates of key points in the original image, The horizontal coordinates of the keypoints output by the YOLO network (located in the input image after Letterbox processing). s represents the vertical coordinates of the keypoints output by the YOLO network (located in the input image after Letterbox processing), and s is the scaling ratio.

[0068] (2) Coarse detection of key points using deep learning. A lightweight YOLO pose / key point detection network (such as YOLOv8-pose, or HRNet, HigherHRNet, or a network with equivalent key point output capability) is preferred, outputting the target category, confidence score, and at least 4 two-dimensional key points. Post-processing filters candidate results with a confidence score below 0.35 and uses non-maximum suppression to remove duplicate detections, preferably 0.45; when there are multiple targets, the target for the current task can be selected according to the strategy of being closest to the center of the screen, having the largest area, or having the highest confidence score.

[0069] (3) Traditional visual subpixel refinement. Based on the bounding region formed by the coarse detection key points of deep learning, the region is expanded outward by about 35% to form a local ROI. The ROI is then subjected to grayscale conversion, 5×5 Gaussian filtering, Canny edge detection (preferably low threshold 50, high threshold 150), 3×3 morphological dilation, main contour screening, and geometric fitting. The corrected corner points are then approximated using the minimum bounding rectangle or polygon and uniformly sorted as top left, top right, bottom right, and bottom left. If the refinement fails, it automatically reverts to the coarse detection key points of deep learning to ensure that the visual link is not interrupted. This two-level strategy enables deep learning to be responsible for robust coarse localization in complex scenes, while traditional vision is responsible for local fine localization.

[0070] (4) Six-DOF pose solution. Establish the correspondence between the three-dimensional points of the target model and the two-dimensional key points of the image. Let the three-dimensional points in the target model coordinate system be:

[0071]

[0072] Homogeneous coordinates of three-dimensional points in the target model coordinate system For the target point in the target model coordinate system Three-dimensional coordinates of direction For the target point in the target model coordinate system Three-dimensional coordinates of direction For the target point in the target model coordinate system The direction represents the three-dimensional coordinates, 1 represents the homogeneous coordinate extension term used for spatial transformation matrix operations, and T represents the transpose operator.

[0073] The two-dimensional key points in the image coordinate system are:

[0074]

[0075] The two-dimensional homogeneous coordinates of the target key points in the image coordinate system. The horizontal pixel coordinates of key points in the image coordinate system, The vertical pixel coordinates of the key points in the image coordinate system are represented by 1, which is the homogeneous coordinate extension term used for perspective projection calculation, and T represents the transpose operator.

[0076] The two satisfy the perspective projection relationship:

[0077]

[0078] Where s is the scale factor in the perspective projection process, K is the camera intrinsic parameter matrix, R is the rotation matrix of the target coordinate system relative to the camera coordinate system, and t is the translation vector of the target coordinate system relative to the camera coordinate system. Together, they constitute the target pose transformation matrix.

[0079] The K-camera intrinsic parameter matrix describes the mapping relationship of 3D points projected from the camera coordinate system to the 2D image coordinate system. It typically takes the following form:

[0080]

[0081] The equivalent focal length of the camera in the horizontal direction, The equivalent focal length of the camera in the vertical direction, The x-coordinate of the principal point of the image, The ordinate of the principal point in the image.

[0082] The rotation matrix of the target coordinate system R relative to the camera coordinate system is as follows:

[0083]

[0084] Its function is to describe the spatial pose changes of the target object relative to the camera. That is, how the target coordinate axis rotates to the camera coordinate axis.

[0085] The translation vector of the target coordinate system relative to the camera coordinate system is in the form of:

[0086]

[0087] It is a 3×1 translation vector used to describe the positional relationship between the origins of two coordinate systems. The position offset of the origin of the target coordinate system in the camera's X direction. The position offset of the origin of the target coordinate system in the camera's Y direction. The position offset of the origin of the target coordinate system in the Z direction of the camera.

[0088] The RANSAC-PnP algorithm is preferably used for solving the problem. It iteratively estimates the rotation vector rvec and translation vector tvec by randomly sampling the correspondence between candidate 3D points and 2D keypoints, and selects valid interior points based on the reprojection error. When the reprojection error is less than a preset threshold, the target pose is re-optimized using all interior points. Preferably, the reprojection error threshold is set to 8 pixels, and the maximum number of iterations is optimally set to 100.

[0089] (5) Pose reliability assessment. The obtained 3D model points are reprojected onto the image plane, and the average Euclidean distance between the predicted pixel points and the measured key points is calculated as the reprojection error err. When err is less than 1 pixel, it is considered highly reliable; when it is 1 to 3 pixels, it is considered usable; and when it is greater than 5 pixels, it is considered unreliable. Unreliable poses are not directly sent to the robotic arm controller, but are instead triggered to re-acquire, re-identify, or manually confirm, thereby preventing erroneous visual poses from entering the execution link.

[0090] (6) Rotation representation and homogeneous matrix construction. The rotation vector output by PnP is converted into a rotation matrix R by the Rodrigues formula, and decomposed into roll, pitch, and yaw in the ZYX order; R is combined with the translation vector t to form a 4×4 homogeneous transformation matrix, thereby unifying the mathematical representation of position and attitude.

[0091] The specific implementation method of coordinate transformation steps is as follows:

[0092] The system involves target coordinate system, camera coordinate system, robotic arm end effector coordinate system, robotic arm base coordinate system, and chassis / world coordinate system. To uniformly describe the position and attitude transformations between these coordinate systems, a 4×4 homogeneous transformation matrix is ​​used to represent the spatial transformation relationships between them. The general form of the homogeneous transformation matrix is:

[0093]

[0094] Where T represents the homogeneous transformation matrix, and R and t, as described above, are used to describe the attitude transformation relationship between the two coordinate systems.

[0095] In the following expressions:

[0096] obj represents the coordinate system of the target object;

[0097] cam represents the camera coordinate system;

[0098] end represents the coordinate system of the end-effector of the robotic arm;

[0099] robot represents the coordinate system of the robotic arm or the robot's coordinate system.

[0100] For a fixed camera mounting method where the "eye is outside the hand," the pose of the target object in the robot's base coordinate system satisfies:

[0101]

[0102] in:

[0103] T_obj_to_robot represents the homogeneous transformation matrix of the target object coordinate system relative to the robot base coordinate system, which is the target pose that is finally output to the robot arm controller. Its translation component represents the three-dimensional position of the target object in the robot base coordinate system, and its rotation component represents the spatial attitude of the target object relative to the robot base coordinate system.

[0104] T_cam_to_robot represents the homogeneous transformation matrix of the camera coordinate system relative to the robot base coordinate system. In the "eye-outside-hand" mounting method, the camera is fixedly mounted on the robot body, chassis, or external fixed bracket. Therefore, this matrix is ​​usually kept fixed after the extrinsic parameter calibration between the camera and the robot or the hand-eye calibration is completed.

[0105] T_obj_to_cam represents the homogeneous transformation matrix of the target object's coordinate system relative to the camera's coordinate system. It is obtained from the aforementioned target key point detection, sub-pixel refinement, and RANSAC-PnP pose solving, and is used to describe the three-dimensional position and pose of the target object in the current camera's field of view.

[0106] For the "eye in hand" camera mounting method, the camera moves with the end effector of the robotic arm, and the pose of the target object in the robot's base coordinate system satisfies:

[0107]

[0108] in:

[0109] T_obj_to_robot represents the final pose of the target object in the robot's base coordinate system, calculated through the aforementioned coordinate transformation chain. This pose is used for subsequent inverse kinematics solving and grasping trajectory planning for the robotic arm.

[0110] T_end_to_robot represents the homogeneous transformation matrix of the robot arm's end-effector coordinate system relative to the robot's base coordinate system. This matrix is ​​obtained through forward kinematics calculations based on the real-time positions of each joint of the robot arm, or it is read in real-time by the robot arm controller; therefore, its value changes in real-time with the movement of the robot arm.

[0111] T_cam_to_end represents the homogeneous transformation matrix of the camera coordinate system relative to the coordinate system of the robotic arm's end effector. Since the camera is fixedly mounted on the end effector, this matrix is ​​obtained through hand-eye calibration on the hand and remains fixed as long as the camera mounting structure does not change.

[0112] T_obj_to_cam represents the homogeneous transformation matrix of the target object's coordinate system relative to the camera's coordinate system, and it is obtained in the same way as in the eye-in-hand mode.

[0113] In the actual calculation process, the homogeneous transformation matrices are multiplied in the order of "gradually transforming from the target object coordinate system to the robot base coordinate system". The system extracts the first three rows and fourth column as the target position from the final T_obj_to_robot:

[0114]

[0115] Where x, y, and z represent the position coordinates of the target object in the X, Y, and Z directions of the robot's base coordinate system, respectively.

[0116] Simultaneously, extract the rotation matrix from the 3×3 submatrix at the top left corner of T_obj_to_robot. It can be converted into the following according to the preset ZYX rotation order:

[0117]

[0118] These represent the roll angle, pitch angle, and yaw angle of the target object relative to the robot's base coordinate system, respectively.

[0119] To avoid systematic positioning errors caused by inconsistencies in units and rotation representations between different modules, this embodiment uniformly converts position coordinates to millimeters (mm) and attitude angles to a certain angle (°) before sending them to the robotic arm motion control module; during the internal calculation of the matrix, the rotation matrix and homogeneous transformation relationship maintain a unified data format and matrix multiplication order.

[0120] Before deployment, camera intrinsic parameters and distortion parameters are calibrated using a multi-view checkerboard pattern. 15 to 30 images of different poses are selected and sub-pixel corner point refinement is employed. In a preferred visual implementation, the lightweight keypoint network inference time is less than 30ms, the local traditional visual refinement time is less than 5ms, the PnP and coordinate transformation time is less than 1ms, and the total latency of the visual guidance and coordinate calculation link can be controlled within 40ms, corresponding to a processing frequency greater than 25fps.

[0121] The specific implementation method of the dynamic compensation step is as follows:

[0122] Based on the target pose output from the second level, the control system calculates the motion trajectory of each joint of the robotic arm by combining the real-time pose of the chassis, the current joint state of the robotic arm, the end-effector load, and the extension direction. When there is a parking deviation of the chassis, ground disturbance, or change in the load of the robotic arm, the controller aligns the chassis state with the robotic arm state through a unified timestamp to form a visual guidance-state feedback compensation.

[0123] Specifically, let the desired pose of the target in the world / robot base coordinate system be T_target, the real-time pose of the chassis be T_base, the fixed transformation of the robot arm base relative to the chassis be T_arm_base, and the current pose of the robot arm end effector be T_tcp. The control system calculates the desired pose of the target relative to the robot arm base coordinate system based on the homogeneous transformation chain, and incorporates the chassis position deviations Δx, Δy, and heading deviation Δθ, along with the robot arm end effector pose error, into a compensation quantity. The robot arm controller then corrects the inverse kinematics of the target and trajectory in real time, preventing chassis errors from being directly added to the end effector.

[0124] In situations where the robot's center of gravity changes, such as when grasping heavy objects, extending the robotic arm significantly, or rotating rapidly, the control system estimates the change in the composite system's center of gravity projection based on the robotic arm's joint angles, end-effector load, and extension direction. When the center of gravity projection approaches a preset stability boundary, the system can reduce the robotic arm's joint speed and acceleration, put the chassis into a braking and holding state, or drive the chassis to make a millimeter-level micro-displacement in the direction opposite to the center of gravity shift, so that the overall center of gravity projection returns to the stable region; the robotic arm continues its movement only after the stability conditions are met. This forms a two-way real-time coordination where "the robotic arm's state affects the chassis control, and the chassis's state corrects the robotic arm's trajectory in the opposite direction," rather than a simple sequential action.

[0125] To avoid asynchronous response speeds between the chassis and the robotic arm, the main control unit employs a unified control cycle to synchronize chassis odometer / positioning data, robotic arm joint states, and visual pose. Ideally, the visual update cycle should not exceed 40ms, and the most recently valid chassis and visual states are used within the robotic arm trajectory control cycle. When the state time difference exceeds the allowable threshold or the visual pose is deemed unreliable, the system pauses the acquisition of new high-precision grasping trajectories and resynchronizes the data.

[0126] Throughout the precision operation phase, the system continuously monitors the chassis pose, visual target pose, robotic arm joint status, and end-effector load changes, and updates the compensation amount according to the aforementioned closed-loop model. When the chassis experiences minor displacement, the robotic arm load causes a change in the overall center of gravity, or the target itself undergoes a small positional change, compensation can be achieved through robotic arm trajectory correction and chassis fine-tuning, thereby improving the overall positioning stability of the composite system.

[0127] The system adopts a unified timestamp synchronization and coordinate system calibration mechanism:

[0128] All sensor data (LiDAR, depth camera, ultrasonic sensor) are tagged with a unified timestamp to ensure the alignment of multi-source data in the time dimension;

[0129] By using hand-eye calibration (between the robotic arm and the camera) and chassis-sensor joint calibration, all sensor data are unified to the same world coordinate system;

[0130] The control system integrates lidar point cloud data, visual image data, and ultrasonic ranging data to form a multimodal comprehensive perception of the environment, thereby improving the accuracy of target recognition and obstacle detection.

[0131] The system is based on the Linux operating system and the ROS2 (Robot Operating System) framework, and adopts a layered modular design.

[0132] Hardware driver layer: responsible for the underlying driving and data acquisition of various sensors, motor drivers, and actuators;

[0133] Algorithm layer: including SLAM mapping algorithm, global and local path planning algorithm, forward and inverse kinematics solving algorithm for robotic arm, visual recognition and localization algorithm, and multi-sensor fusion algorithm;

[0134] Business logic layer: responsible for task scheduling, status monitoring, fault diagnosis, and logical decision-making;

[0135] Upper computer interaction layer: Provides a human-computer interaction interface and supports three modes: manual remote control, semi-automatic point operation, and fully automatic program operation.

[0136] It is powered by a 24V ternary lithium battery pack with a rated capacity of 20Ah. It integrates a BMS battery management system and has overvoltage, undervoltage, overcurrent, overheat, short circuit, and overcharge protection functions.

[0137] Equipped with a 24V to 12V step-down module, it provides a stable power supply for various sensors and control boards, and has overvoltage, overcurrent, overheat, and short-circuit protection.

[0138] The total emergency stop response time for the entire machine is ≤500ms.

[0139] In one specific embodiment, the hardware configuration of the system of the present invention is as follows:

[0140] Mobile chassis: Differential drive chassis, with overall dimensions of 600mm (length) × 600mm (width) × 300mm (height), maximum load of 10kg, maximum moving speed of 3.7m / s (adjustable), positioning accuracy of ±10mm, climbing ability of ≤30°, and obstacle crossing height of 100mm.

[0141] Robotic arm: Six-degree-of-freedom robotic arm, maximum working radius 1200mm, rated load 10kg, repeatability ±10mm, end effector is gripper, and the movement speed of each joint is adjustable;

[0142] LiDAR: Installed inside the AMR chassis at a height of 200-400mm from the ground, used for environmental 2D contour scanning and SLAM mapping;

[0143] Depth camera: mounted on the front of the body and on the robotic arm, used to acquire color images and depth information to achieve target recognition and grasping coordinate calculation;

[0144] Ultrasonic sensors: Multiple rings are arranged around the body, 150-250mm above the ground, for detecting transparent objects and low obstacles;

[0145] Safety edge: Installed at the bottom outermost ring around the chassis to protect the machine body when the chassis is crossing obstacles;

[0146] Emergency stop switch: Installed in a conspicuous position on the side and top of the machine body, with a mushroom-shaped button and normally closed contacts connected to a safety hard circuit;

[0147] Power supply system: 24V / 20Ah ternary lithium battery pack, with integrated BMS protection board.

[0148] In one specific embodiment, the collaborative operation control process of the present invention is as follows:

[0149] Step S1: The control system receives the work task instruction (such as "go to point A to grab material X and transport it to point B").

[0150] Step S2: The mobile chassis performs global path planning based on the SLAM map and autonomously navigates to the target point A;

[0151] Step S3: After the chassis reaches the target point A, perform precise docking, lock the position, and control the positioning accuracy within ±10mm;

[0152] Step S4: The vision module acquires images of the work area, preprocesses them using Letterbox at the same scale, and then performs coarse target localization using a key point detection network. Within the local ROI, it performs sub-pixel corner point refinement using edge detection, contour extraction, and geometric fitting. Based on the two-dimensional key points and the target's three-dimensional model points, RANSAC-PnP is performed to obtain the target's six-degree-of-freedom pose in the camera coordinate system, and the credibility is judged by the reprojection error.

[0153] Step S5: Perform coordinate transformation according to the camera mounting method. When the eye is outside the hand:

[0154]

[0155] Use when the eye is on the hand:

[0156]

[0157] The target pose is converted into a unified pose in the robot arm base / robot coordinate system; then the initial compensation trajectory of the robot arm is calculated based on the actual parking pose of the chassis.

[0158] During the approach, grasping and transport of the robotic arm, the chassis pose, the joint status of the robotic arm and the end effector load are continuously read. When the chassis micro-displacement or the extension / load of the robotic arm causes the center of gravity to shift, the control system synchronously corrects the trajectory of the robotic arm and selects chassis braking, speed limiting or millimeter-level micro-displacement according to the stability margin, so that the chassis and the robotic arm form a two-way real-time collaboration.

[0159] Step S6: The robotic arm performs a gripping action, and the end gripper closes to complete the material gripping;

[0160] Step S7: The robotic arm lifts and rotates the material to a safe position, and the chassis restarts navigation to proceed to the target location B;

[0161] Step S8: After reaching the target point B, repeat steps S3-S5. The robotic arm performs the placement action, and the end gripper opens to complete the material placement.

[0162] Step S9: After the operation is completed, the equipment returns to the standby position and waits for the next task instruction.

[0163] Throughout the entire operation process, the system monitors the following safety statuses in real time:

[0164] 1. The lidar and ultrasonic sensors continuously detect obstacles on the travel path, and automatically slow down or detour when encountering obstacles;

[0165] 2. If a collision occurs due to the non-contact sensor failing to detect an obstacle, the safety contact edge will be pressed, triggering an immediate shutdown.

[0166] 3. In any abnormal situation, the operator can press the emergency stop switch, and the hardware circuit will directly cut off the power.

[0167] 4. The system collects battery voltage, current, temperature and other status information in real time, and issues audible and visual alarms when abnormalities occur.

[0168] In one specific embodiment, the software system of the present invention is developed based on the Ubuntu operating system and the ROS2 framework. Each functional module (navigation module, chassis control module, robotic arm control module, vision recognition module, status monitoring module, and communication module) runs as an independent node and performs data interaction and collaborative scheduling through the ROS2 topic communication and service call mechanism.

[0169] In terms of communication, the system supports TCP communication, I2C communication, CAN communication, UART serial communication (baud rate 115200, data bits 8 bits, stop bits 1 bit, no parity), WiFi communication (IP address 192.168.11.1, port 1445) and Ethernet communication, and can be connected to host computers, remote controls and various external devices.

[0170] The above description is only a specific embodiment of the present invention. Various examples and illustrations do not constitute a limitation on the substantive content of the present invention. Those skilled in the art can make modifications or variations to the above-described specific embodiments after reading the specification without departing from the substance and scope of the invention.

Claims

1. A method for operating a mobile robotic arm based on vision guidance and dynamic compensation, characterized in that: Includes the following steps, The mobile chassis autonomously plans its route according to mission instructions and drives to the target location for parking. The vision module acquires images of the work area, performs coarse detection of key points of the target object through a deep learning network, and constructs a local region of interest based on the enclosing region formed by the coarsely detected key points. Within the local region of interest, sub-pixel level corner point refinement is performed using traditional vision methods to obtain the corrected corner point coordinates. Then, based on the correspondence between the corrected corner point coordinates and the points of the 3D model of the target object, the six-degree-of-freedom pose of the target object is solved. Based on the camera mounting method, the pose of the target object in the camera coordinate system is transformed to the robot base coordinate system; During the operation of the robotic arm, the control system acquires the real-time posture of the chassis, the current joint status of the robotic arm and the end-effector load information, and generates compensation based on the chassis posture deviation and the robotic arm status to correct the movement trajectory of the robotic arm in real time. When the deep learning network fails to perform coarse detection, the coarse detection result is used as the output directly; when the subpixel refinement fails, it automatically reverts to the deep learning coarse detection result.

2. The mobile robotic arm operation method based on vision guidance and dynamic compensation as described in claim 1, characterized in that: The vision module acquires images of the work area. After acquisition, the images are scaled proportionally and filled with black borders using Letterbox. The scaling ratio is... Where W is the width of the original input image, H is the height of the original input image, Wt is the width of the network input size, Ht is the height of the network input size, and the horizontal offset is recorded. and vertical offset After network output, through =( - ) / s、 =( - ) / s maps keypoints back to their original image coordinates, where , To recover the key point coordinates from the original image, , These are the coordinates of key points output by the deep learning network.

3. The mobile robotic arm operation method based on vision guidance and dynamic compensation as described in claim 1, characterized in that: The deep learning network employs a lightweight keypoint detection network, which outputs the target category, confidence score, and at least four two-dimensional keypoints. Post-processing filters candidate results with confidence scores below 0.35 and uses non-maximum suppression to remove duplicate detections. The non-maximum suppression threshold is set to 0.

45.

4. The mobile robotic arm operation method based on vision guidance and dynamic compensation as described in claim 1, characterized in that: Based on the enclosed region formed by the coarse detection key points of deep learning, a local region of interest is formed by expanding outward by 35%. The local region of interest is then subjected to grayscale conversion, Gaussian filtering, Canny edge detection, morphological dilation, main contour screening and geometric fitting in sequence. The corrected corner points are obtained by approximating with the minimum bounding rectangle or polygon and are uniformly sorted in the order of top left, top right, bottom right and bottom left.

5. The mobile robotic arm operation method based on vision guidance and dynamic compensation as described in claim 1, characterized in that: The RANSAC-PnP algorithm is used to solve the six-DOF pose of the target object. The reprojection error threshold is set to 8 pixels and the maximum number of iterations is set to 100. After solving, the reprojection error is calculated. When the reprojection error is less than 1 pixel, it is considered highly reliable. When it is between 1 and 3 pixels, it is considered usable. When it is greater than 5 pixels, it is considered unreliable. Unreliable poses are not sent to the robotic arm controller.

6. The mobile robotic arm operation method based on vision guidance and dynamic compensation as described in claim 1, characterized in that: For a camera mounting method where the eye is outside the hand, the pose of the target object in the robot's base coordinate system must satisfy... For the camera mounting method with the eye on the hand, the pose of the target object in the robot's base coordinate system satisfies Where T represents the homogeneous transformation matrix, obj represents the target object coordinate system, cam represents the camera coordinate system, end represents the end effector coordinate system, and robot represents the robot base coordinate system.

7. The mobile robotic arm operation method based on vision guidance and dynamic compensation as described in claim 1, characterized in that: The control system estimates the projected position of the center of gravity of the composite system based on the joint angle of the robotic arm, the end load, and the extension direction. When the projected center of gravity approaches the preset stable boundary, the system reduces the joint speed and acceleration of the robotic arm, or drives the chassis to make a millimeter-level micro-displacement in the direction opposite to the center of gravity offset, so that the projected center of gravity returns to the stable region.

8. The mobile robotic arm operation method based on vision guidance and dynamic compensation as described in claim 1, characterized in that: It also includes safety protection measures: when the non-contact sensor detects an obstacle, the robot is controlled to slow down or go around it; when the non-contact sensor fails to detect an obstacle and a collision occurs, the contact safety edge is squeezed and outputs a trigger signal to immediately trigger the shutdown protection; when the operator presses the emergency stop switch, the hardware circuit directly cuts off the power; the non-contact sensor includes lidar, depth camera and ultrasonic sensor.

9. The mobile robotic arm operation method based on vision guidance and dynamic compensation as described in claim 1, characterized in that: The control system uses a unified control cycle to synchronize chassis positioning data, robotic arm joint status and visual pose in time, with a visual update cycle of no more than 40ms. When the time difference between chassis status and visual pose exceeds the allowable threshold or the visual pose is determined to be unreliable, the execution of new grasping trajectory is paused and the data is resynchronized.

10. A mobile robotic arm operation system based on vision guidance and dynamic compensation, characterized in that: include: The mobile chassis module performs the chassis autonomous navigation step in the method according to any one of claims 1 to 9; The robotic arm operation module is installed on the mobile chassis. It is a six-degree-of-freedom robotic arm with a gripper or suction cup at the end, and performs the operation actions in the method of any one of claims 1 to 9. The multimodal perception module includes a lidar, a depth camera, an ultrasonic sensor, and a contact-type safety edge, and performs the visual guidance step and the safety protection step in the method of any one of claims 1 to 9; The control system module is communicatively connected to the mobile chassis module, the robotic arm operation module, and the multimodal perception module, respectively, and executes the coordinate transformation step and the dynamic compensation step in the method of any one of claims 1 to 9.

Images