Hump unhooking robot cooperative following positioning and end control method and system

Abstract

The application relates to the technical field of railway transportation control, and provides a hump uncoupling robot cooperative following positioning and end control method and system. The method comprises the following steps: obtaining a target uncoupling position and operation time sequence of a to-be-uncoupled train, controlling a wheeled mobile chassis to dynamically follow the to-be-uncoupled train along a trackside path; obtaining a following error estimation result based on relative motion information and an extended Kalman filter; identifying a hook lifting rod key point through an RGB-D camera and an improved YOLOv5s detection model, and determining a hook lifting rod space position in a robot coordinate system; and combining the following error and the visual positioning error to judge an end execution condition, and executing an uncoupling action when the condition is met. The application can uniformly judge the robot following state, the hook lifting rod positioning state and the end execution timing, reduces the influence of misexecution and positioning deviation on the uncoupling action, and improves the automation degree, safety and reliability of the uncoupling operation.

Description

Technical Field

[0001] This application relates to the field of railway transportation control technology, specifically to a collaborative following positioning and end-effector control method and system for a camel hump unhooking robot. Background Technology

[0002] In recent years, railway automation and intelligent technologies have continued to develop. In hump yards, processes such as peak pushing, shunting, and turnout control have been gradually automated. However, the uncoupling and coupling operations during train dismantling still rely heavily on manual labor or remain in the research stage of fixed and track-mounted auxiliary uncoupling devices.

[0003] Hump ​​uncoupling operations are characterized by their dynamic nature. The train of cars to be uncoupled is in continuous motion during the pushing and shunting process. The target uncoupling position needs to be matched with the dismantling operation sequence. The position of the hook lifting rod is also affected by the movement of the train, vehicle swaying, robot driving deviation, and the on-site environment.

[0004] Existing automated uncoupling solutions mostly focus on coupler area recognition or end-of-line hook lifting, but do not adequately consider the relative position changes during continuous train movement, the spatial position changes of the hook lifting rod, and the reliability of the execution action. This leads to the uncoupling robot being prone to end-of-line deviations in dynamic operation scenarios, resulting in uncoupling failure and affecting the reliability of hump uncoupling operations.

[0005] Therefore, improving the reliability of hump uncoupling operations in the context of continuous train movement has become a technical problem that needs to be solved. Summary of the Invention

[0006] This application aims to solve the problems in the prior art and provide a highly reliable method and system for collaborative following, positioning and end-effector control of a hump unhooking robot.

[0007] To achieve the above objectives, the first aspect of this application provides a method for cooperative following, localization, and end-effector control of a hump-unhooking robot, comprising:

[0008] The target uncoupling position and corresponding uncoupling operation sequence of the train to be uncoupled are obtained. Based on the target uncoupling position and the uncoupling operation sequence, the hump uncoupling robot is controlled to travel along the trackside path, so that the wheeled mobile chassis of the hump uncoupling robot dynamically follows the train to be uncoupled. During the dynamic following process, based on the relative motion information between the wheeled mobile chassis and the train to be uncoupled, the longitudinal following error and the lateral following error of the wheeled mobile chassis are determined, and the longitudinal following error and the lateral following error are estimated based on the extended Kalman filter to obtain the following error estimation result. The image and depth data of the coupler area of ​​the train to be uncoupled are acquired by an RGB-D camera, and the image of the coupler area is input into an improved YOLOv5s detection model to identify the image position of the key points of the coupler lifting rod. Based on the image position of the hook lifting rod key point and the depth value corresponding to the image position in the depth data, the three-dimensional position of the hook lifting rod key point in the camera coordinate system is determined, and based on the calibration relationship between the camera coordinate system and the robot coordinate system, the three-dimensional position is converted into the hook lifting rod spatial position in the robot coordinate system. The target pose of the robotic arm end effector is determined based on the spatial position of the hook lifting rod, and the execution conditions of the target pose of the robotic arm end effector are jointly constrained and judged based on the following error estimation result and the visual positioning error of the hook lifting rod corresponding to the spatial position of the hook lifting rod. If both the following error estimation result and the hook lifting rod visual positioning error meet the preset execution conditions, the end effector of the robotic arm of the camel hump unhooking robot is controlled to perform the unhooking action according to the target pose of the end effector of the robotic arm; if at least one of the following error estimation result and the hook lifting rod visual positioning error does not meet the preset execution conditions, the execution action of the end effector of the robotic arm is paused.

[0009] The second aspect of this application provides a collaborative following, positioning, and end-effector control system for a camel hump unhooking robot, comprising: The system includes a hump unhooking robot, an RGB-D camera, and a controller. The hump unhooking robot comprises a wheeled mobile chassis and a robotic arm end effector mounted on the mobile chassis. The controller is communicatively connected to the wheeled mobile chassis, the RGB-D camera, and the robotic arm end effector. The controller is used to obtain the target uncoupling position and the corresponding uncoupling operation sequence in the train to be uncoupled, and based on the target uncoupling position and the uncoupling operation sequence, control the wheeled mobile chassis to travel along the trackside path so that the wheeled mobile chassis dynamically follows the train to be uncoupled. The controller is also used to determine the longitudinal and lateral following errors of the wheeled mobile chassis based on the relative motion information between the wheeled mobile chassis and the train to be uncoupled and the trackside reference path during the dynamic following process, and to perform state estimation on the longitudinal and lateral following errors based on extended Kalman filtering to obtain the following error estimation result. The RGB-D camera is used to acquire images and depth data of the coupler area of ​​the train to be uncoupled. The controller is also used to input the coupler area image into the improved YOLOv5s detection model to identify the image position of the key points of the coupler lifting rod. The controller is also used to determine the three-dimensional position of the hook lifting rod key point in the camera coordinate system based on the image position of the hook lifting rod key point and the depth value corresponding to the image position in the depth data, and to convert the three-dimensional position into the hook lifting rod spatial position in the robot coordinate system according to the calibration relationship between the camera coordinate system and the robot coordinate system. The controller is also used to determine the target pose of the end effector of the robotic arm based on the spatial position of the hook lifting rod, and to jointly constrain and judge the execution conditions of the target pose of the end effector of the robotic arm based on the following error estimation result and the visual positioning error of the hook lifting rod corresponding to the spatial position of the hook lifting rod. The controller is further configured to, when both the following error estimation result and the hook lifting rod visual positioning error meet the preset execution conditions, control the end effector of the robotic arm of the hump unhooking robot to perform the unhooking action according to the target pose of the end effector of the robotic arm; and to pause the execution action of the end effector of the robotic arm when at least one of the following error estimation result and the hook lifting rod visual positioning error does not meet the preset execution conditions.

[0010] A third aspect of this application provides a terminal device, including a processor and a memory, wherein the memory stores a computer program that, when executed by the processor, implements the method described above.

[0011] Compared with the prior art, this application has the following beneficial effects: By combining dynamic following control of a wheeled mobile chassis, RGB-D visual positioning, improved YOLOv5s key point detection, and end-effector control, the hump unhooking robot can maintain stable following during the movement of the train to be unhooked, and perform unhooking actions while meeting the requirements of following accuracy and visual positioning accuracy. This reduces the safety risks in manual unhooking operations and improves the automation and reliability of unhooking actions. Attached Figure Description

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

[0013] Figure 1 This is a flowchart illustrating a method for collaborative following, positioning, and end-effector control of a camel hump unhooking robot according to an embodiment of this application. Figure 2 This is a schematic diagram of the structure of a collaborative following positioning and end-effector control system for a camel hump unhooking robot provided in an embodiment of this application; Figure 3 This is a schematic diagram of the structure of a terminal device provided in an embodiment of this application. Detailed Implementation

[0014] To facilitate understanding of this application, the following description will be more comprehensive and detailed in conjunction with the accompanying drawings and preferred embodiments, but the scope of protection of this application is not limited to the following specific embodiments.

[0015] Unless otherwise defined, all technical terms used herein have the same meaning as commonly understood by those skilled in the art. The technical terms used herein are for the purpose of describing particular embodiments only and are not intended to limit the scope of this application.

[0016] Please see Figure 1 One embodiment of this application provides a method for cooperative following, positioning, and end-effector control of a hump-unhooking robot, comprising the following steps: S102. Obtain the target uncoupling position and corresponding uncoupling operation sequence of the train to be uncoupled. Based on the target uncoupling position and uncoupling operation sequence, control the hump uncoupling robot to travel along the trackside path, so that the wheeled mobile chassis of the hump uncoupling robot dynamically follows the train to be uncoupled.

[0017] In this step, the target uncoupling position can be determined based on the hump dismantling plan, hump pushing plan, or uncoupling operation sequence, and is used to indicate the position between adjacent vehicles in the train to be uncoupled. The uncoupling operation sequence is used to indicate the time order in which the target uncoupling position arrives at the robot's working area and the timing of execution. After receiving the target uncoupling position and the uncoupling operation sequence, the hump uncoupling robot travels along the trackside reference path, ensuring that the wheeled mobile chassis maintains a relative position to the target uncoupling position during the train's movement.

[0018] S104. During the dynamic following process, based on the relative motion information between the wheeled mobile chassis and the train to be uncoupled and the trackside reference path, the longitudinal following error and the lateral following error of the wheeled mobile chassis are determined, and the state estimation of the longitudinal following error and the lateral following error is performed based on the extended Kalman filter to obtain the following error estimation result.

[0019] In this step, the relative motion information includes the speed difference and position difference between the wheeled mobile chassis and the train to be uncoupled, as well as the offset of the wheeled mobile chassis relative to the trackside reference path. The longitudinal following error represents the degree to which the wheeled mobile chassis is ahead or behind the target uncoupling position along the direction of train movement, while the lateral following error represents the degree of lateral offset of the wheeled mobile chassis relative to the trackside reference path or track centerline. The longitudinal following error can be expressed by the following formula:

[0020] in, This indicates that the camel hump unhooking robot is at a certain time. instantaneous speed, Indicates the time when the train to be uncoupled is... instantaneous speed, Indicates time The longitudinal following error. The lateral following error. This can be the vertical distance from the geometric center of the wheeled mobile chassis to the trackside reference path or the track centerline. By estimating the error using extended Kalman filtering, the impact of sensor noise, chassis slippage, and train speed fluctuations on the following judgment can be reduced.

[0021] S106. Acquire images and depth data of the coupler area of ​​the train to be uncoupled using an RGB-D camera, and input the coupler area images into the improved YOLOv5s detection model to identify the image positions of key points on the coupler lifting rod.

[0022] In this step, the RGB-D camera can be positioned on the robot body, the end effector of the robotic arm, or at a lateral sensing location to simultaneously acquire color images and corresponding depth images of the coupler area. When the RGB-D camera is fixedly positioned on the robot body or at a lateral sensing location, the calibration relationship between the camera coordinate system and the robot coordinate system can be a fixed calibration relationship. When the RGB-D camera is positioned at the end effector of the robotic arm, the coordinate transformation relationship between the camera coordinate system and the robot coordinate system can be determined jointly based on the current pose of the end effector of the robotic arm and the installation calibration relationship of the camera relative to the end effector of the robotic arm. After the coupler area image is input into the improved YOLOv5s detection model, the model detects the area where the hook rod is located and outputs the pixel coordinates of the hook rod key points in the image. The hook rod key points can include the center of the hook rod's rotation axis and the handle end point, where the handle end point can serve as an important position for the alignment or force application of the robotic arm end effector.

[0023] S108. Based on the image position of the key point of the hook lifting rod and the depth value corresponding to the image position in the depth data, determine the three-dimensional position of the key point of the hook lifting rod in the camera coordinate system, and convert the three-dimensional position into the spatial position of the hook lifting rod in the robot coordinate system according to the calibration relationship between the camera coordinate system and the robot coordinate system.

[0024] In this step, after obtaining the pixel coordinates of the key points of the hook lifting rod, the depth value corresponding to the pixel coordinates can be read from the depth data, and the 2D image position can be back-projected into a 3D position in the camera coordinate system using camera intrinsic parameters. The calculation can be performed using the following pinhole camera model:

[0025] in, The image coordinates of the key points of the hook lifting rod. This represents the depth value corresponding to the image coordinates. Indicates camera intrinsic parameters. This represents the three-dimensional coordinates of the hook lifting rod's key points in the camera coordinate system. A coordinate transformation is performed based on the hand-eye calibration relationship between the camera and robot coordinate systems to obtain the hook lifting rod's spatial position in the robot coordinate system. This transformation can be expressed as:

[0026] in, This indicates the position of the key point of the hook lifting rod in the camera coordinate system. This represents the rigid body transformation matrix from the camera coordinate system to the robot coordinate system. This indicates the position of the key point of the hook lifting rod in the robot coordinate system.

[0027] S110. Determine the end-effector pose of the robotic arm based on the spatial position of the hook and lever, and make joint constraint judgments on the execution conditions of the end-effector pose based on the following error estimation results and the visual positioning error of the hook and lever corresponding to the spatial position of the hook and lever.

[0028] In this step, the target pose of the robotic arm's end effector can be determined based on the spatial position of the hook lifting rod, the structural form of the end effector, and the direction of the hook removal action. For example, when the end effector is used to pull or push the hook lifting rod, the alignment direction, approach position, and force application posture can be determined based on the center of the hook lifting rod's pivot and the end of the handle. The visual positioning error of the hook lifting rod can be used to represent the deviation between the visually recognized position and the actual position of the hook lifting rod, and it can be expressed as:

[0029] in, This indicates the identified hook and lever position. Indicates the actual position of the hook and rod. This indicates the visual positioning error of the hook lifting rod. Visual positioning error can also be composed of image detection error, depth error, and calibration error.

[0030] In addition, during the training or calibration phase, the visual positioning error of the hook lifting rod can be determined based on the deviation between the identified hook lifting rod position and the actual hook lifting rod position obtained by annotation or measurement; during the online hook removal phase, the visual positioning error of the hook lifting rod can be estimated based on the image detection error, depth measurement error, and calibration error.

[0031] S112. When both the following error estimation result and the hook lifting rod visual positioning error meet the preset execution conditions, control the end effector of the robotic arm of the camel hump unhooking robot to perform the unhooking action according to the target pose of the end effector of the robotic arm. If at least one of the following error estimation result and the visual positioning error of the hook lifting rod does not meet the preset execution conditions, the execution action of the end effector of the robotic arm is paused; wherein, if the following error estimation result does not meet the preset execution conditions, the wheeled mobile chassis is controlled to perform following correction; if the visual positioning error of the hook lifting rod does not meet the preset execution conditions, the image of the coupler area and the depth data are re-acquired to update the spatial position of the hook lifting rod.

[0032] In this step, the preset execution conditions may include a following error threshold and a visual positioning error threshold. When the positional error of the wheeled mobile chassis relative to the train to be uncoupled is within the allowable range, and the visual positioning accuracy of the hook lifting rod meets the alignment requirements of the robotic arm, the end effector of the robotic arm approaches the hook lifting rod according to the target pose and performs the uncoupling action. When the following error estimation result does not meet the preset execution conditions, it indicates that there is a large deviation between the relative position of the wheeled mobile chassis and the target uncoupling position. At this time, pausing the action of the end effector of the robotic arm can reduce the risk of the robotic arm accidentally hitting the train body or missing the hook lifting rod. When the visual positioning error of the hook lifting rod does not meet the preset execution conditions, it indicates that the current visual positioning result is unreliable. At this time, re-acquiring the image and depth data of the train coupler area and updating the spatial position of the hook lifting rod can improve the accuracy of the alignment of the end effector of the robotic arm.

[0033] In this embodiment, by combining dynamic following control of the wheeled mobile chassis, RGB-D visual positioning, improved YOLOv5s key point detection, and robotic arm end effector control, the hump unhooking robot can maintain stable following during the movement of the train to be unhooked, and perform the unhooking action while meeting the requirements of following accuracy and visual positioning accuracy. This reduces the safety risks of manual unhooking operations and improves the automation level and reliability of the unhooking action.

[0034] In one embodiment, state estimation is performed on the longitudinal and lateral following errors based on extended Kalman filtering to obtain the following following error estimation results: A following state vector is constructed using longitudinal following error, lateral following error, rate of change of longitudinal following error, and rate of change of lateral following error. The following state vector is predicted based on the acceleration and steering angular velocity of the wheeled mobile chassis to obtain the predicted state; Based on the observation information obtained by at least one of the following sensors—a wheel encoder, an inertial measurement unit, and a lidar—the predicted state is updated to obtain the following error estimation result, and the covariance information corresponding to the following error estimation result is obtained. Among them, the wheel encoder and the inertial measurement unit are used to obtain chassis motion information, and the lidar is used to obtain the relative pose information of the vehicle train or the relative pose information of the trackside path.

[0035] Specifically, the following state vector can be represented as:

[0036] in, This represents the longitudinal following error. This indicates the lateral following error. This represents the rate of change of the longitudinal following error. This represents the rate of change of the lateral following error. Control inputs can include the acceleration and steering angular velocity of the wheeled mobile chassis, and can be expressed as:

[0037] At discrete time intervals, the state at the next time interval can be predicted based on the state vector of the previous time interval and the control input. Since there is a nonlinear relationship between the motion state of the wheeled mobile chassis and its acceleration and steering angular velocity, the extended Kalman filter can linearize the kinematic model near the current estimated state. The linearized discretized state equation can be expressed as:

[0038] in, This represents the state transition matrix obtained by linearizing the kinematic model. This represents the input control matrix obtained by linearizing the control input terms. Indicates control input, This represents process noise. The observation equation between sensor observations and the state vector can be expressed as:

[0039] in, Represents sensor observations, This represents the observation matrix obtained by linearizing the observation model. This represents observation noise. The extended Kalman filter first predicts based on the control input, then updates it based on data from at least one sensor among the wheel encoder, inertial measurement unit, and lidar, thus obtaining a relatively stable tracking error estimate. The corresponding covariance information can be used to represent the uncertainty of the tracking error estimate, ensuring that subsequent robotic arm decisions consider not only the error value itself but also the reliability of the error estimate.

[0040] In one embodiment, the common constraint determination of the execution conditions for the target pose of the robotic arm end effector includes: Based on the following error estimation results and the covariance information of the following state vector, the following error range corresponding to the robot arm end effector reaching the target pose of the robot arm end is predicted; If the predicted following error range meets the preset following error threshold, and the hook lifting rod visual positioning error meets the preset visual positioning error threshold, then the preset execution condition is determined to be met.

[0041] Specifically, it takes a certain amount of time for the end effector of the robotic arm to move from its current position to the target pose. During this time, there may still be relative motion between the wheeled mobile chassis and the unhooking vehicle train. The following error range when the end effector reaches the target pose can be predicted based on the current following error estimation results, error change rate, and covariance information. If both the predicted longitudinal and lateral errors are within a preset following error threshold, and the visual positioning error of the hook lifting rod is within a preset visual positioning error threshold, then it is considered that the end effector of the robotic arm can still accurately align with the hook lifting rod when reaching the target pose. The preset following error threshold can be set with reference to the following control target settings:

[0042] The visual positioning error of the hook lifting rod meets the following requirements: ; By using common constraints, we can avoid making judgments based solely on visual positioning results or chassis following errors. This ensures that the robotic arm's end effector matches the chassis following state and the reliability of visual positioning, thereby improving the safety and success rate of the dynamic unhooking process.

[0043] In one embodiment, the improved YOLOv5s detection model includes a keypoint detection branch, which is used to output the image positions of the pivot center and the handle end of the hook rod. The improved YOLOv5s detection model incorporates object classification loss, bounding box regression loss, and keypoint location loss, enabling it to identify the hook and lever keypoints used for end-effector alignment while recognizing the hook and lever's location.

[0044] Specifically, the improved YOLOv5s detection model can take RGB images as input, identify the area where the hook lever is located through the object detection branch, and output the image coordinates of the center of the hook lever's pivot and the end of the handle through the key point detection branch. Compared to the object detection model that only outputs the detection box, the key point detection branch can further determine the specific action position that the robotic arm's end effector needs to be aligned with.

[0045] The training loss of the improved YOLOv5s detection model can be expressed as:

[0046] in, This represents the target classification loss, used to determine the target category; This represents the bounding box regression loss, used to constrain the positional deviation between the detected box and the ground truth box; This represents the keypoint position loss, used to constrain the positional deviation between predicted and labeled keypoints. By simultaneously training object detection and keypoint localization, the improved YOLOv5s detection model can identify the hook lifting rod in complex coupler regions and output the positions of the pivot center and handle end point.

[0047] In one embodiment, determining the three-dimensional position of the hook lifting rod keypoint in the camera coordinate system based on the image position and depth value corresponding to the image position in the depth data includes: Align the coupler area image with the depth data pixel by pixel; Filter valid depth values ​​within the neighborhood of the image location of the key point of the hook lifting rod; When the depth value corresponding to the image location is missing or fluctuates beyond the preset range, the effective depth value is filtered and fused by combining depth data from multiple consecutive frames to obtain a corrected depth value; the selected effective depth value or corrected depth value is used as the target depth value. Based on the image position of the key points of the hook lifting rod, the target depth value, and the intrinsic parameters of the RGB-D camera, determine the three-dimensional position of the key points of the hook lifting rod in the camera coordinate system.

[0048] Specifically, the image and depth data of the coupler area acquired by the RGB-D camera need to be pixel-level aligned so that the key points of the hook and lifting rod in the color image can correspond to the same spatial position in the depth map. After improving the image position of the key points of the hook and lifting rod output by the YOLOv5s detection model, a neighborhood range can be set around this image position, and invalid depth values, abnormally abrupt depth values, or depth values ​​that deviate significantly from the surrounding depth distribution can be removed from the neighborhood range to obtain the effective depth value. Due to the presence of metal reflection, occlusion, and missing edge depth in the coupler area, single-frame depth data may have holes or fluctuations. The effective depth values ​​can be filtered and fused by combining depth data from multiple consecutive frames to obtain a more stable corrected depth value. If the effective depth value of the current frame is reliable, it can be directly used as the target depth value; if the depth of the current frame is missing or fluctuates greatly, the corrected depth value is used as the target depth value. The image position of the key points of the hook and lifting rod and the target depth value are substituted into the pinhole camera model to calculate the three-dimensional position of the key points of the hook and lifting rod in the camera coordinate system. It can reduce the impact of depth loss or depth jitter on the spatial positioning results of the hook lifting rod and improve the stability of the alignment of the robotic arm end effector.

[0049] In one embodiment, the visual positioning error of the hook lifting rod is determined based on image detection error, depth measurement error, and calibration error; Among them, the image detection error is the spatial error corresponding to the position error of the hook lifting rod key point image output by the improved YOLOv5s detection model, the depth measurement error is the depth error of the RGB-D camera at the hook lifting rod key point, and the calibration error is the calibration error between the camera coordinate system and the robot coordinate system.

[0050] Specifically, the visual positioning error of the hook lifting rod is jointly generated by image recognition, depth measurement, and coordinate transformation. Image detection error mainly manifests as the deviation between the predicted and actual positions of key points such as the hook lifting rod's pivot center and handle endpoint in the image. This deviation is converted into a spatial position error after depth back projection. Depth measurement error mainly originates from the ranging error of the RGB-D camera at the depth values ​​of key points on the hook lifting rod; for example, metal reflection, edge occlusion, or holes in the depth map can all cause unstable depth values. Calibration error mainly originates from camera intrinsic parameter calibration and hand-eye calibration errors between the camera coordinate system and the robot coordinate system. The visual positioning error of the hook lifting rod can be represented by the following total error model: Its total error model can be expressed as:

[0051] in, Indicates image detection error. Indicates depth measurement error. This indicates calibration error. By incorporating these three types of errors into the execution judgment, the unhooking action of the robotic arm's end effector can be paused in a timely manner when key point recognition in the image is unstable, depth data is abnormal, or calibration accuracy is insufficient, thereby reducing the risk of erroneous execution.

[0052] In one embodiment, the target pose of the robotic arm end effector includes a pre-alignment pose located outside the spatial position of the hook lifting rod and an execution pose for acting on the hook lifting rod; controlling the robotic arm end effector to perform the hook removal action according to the target pose of the robotic arm end effector includes: Control the end effector of the robotic arm to move to the pre-aligned position; if the following error estimation result and the continuous prediction time of the hook lifting rod visual positioning error meet the preset execution conditions, control the end effector of the robotic arm to move from the pre-aligned position to the execution position and perform the hook removal action.

[0053] Specifically, the pre-alignment posture can be set outside the hook lifting lever's spatial position, allowing the robotic arm's end effector to first reach a position close to the hook lifting lever but not yet directly acting on it. The execution posture can be determined based on the hook lifting lever's pivot center, handle end point, and the pulling or pushing direction required for hook removal, ensuring effective contact between the robotic arm's end effector and the hook lifting lever to complete the hook removal action. By employing a segmented control method for the pre-alignment and execution postures, the robotic arm's end effector can first enter a safer approach position, and then, with reliable chassis following and visual positioning, perform the final execution action.

[0054] After the robotic arm's end effector moves to the pre-aligned position, it can continue to monitor the tracking error estimation result and the hook lifting rod visual positioning error. If both meet the preset execution conditions for a preset duration, the robotic arm's end effector is controlled to move from the pre-aligned position to the execution position and complete the hook removal action. If either condition is not met, execution can be paused, and tracking correction or visual positioning update can be performed again. In this way, the robotic arm can avoid directly acting on the hook lifting rod when it is not fully aligned or the chassis tracking is unstable, thus improving the safety and reliability of the hook removal action.

[0055] In one embodiment, during the process of controlling the end effector of the robotic arm to move from the pre-aligned position to the execution position, obstacle avoidance constraints are generated based on the car body structure, coupler area structure, and hook lifting rod spatial position of the train to be uncoupled; when the distance between the planned path and the car body structure or coupler area structure is less than the preset safety distance, the motion path of the end effector of the robotic arm is replanned or the uncoupling action is paused.

[0056] Specifically, when the end effector of the robotic arm moves from the pre-aligned position to the execution position, its motion path may pass near the edge of the vehicle body, the coupler body, the structure around the hook lifting rod, or other protruding components. Therefore, the vehicle body structure and coupler area structure of the train to be coupled can be obtained based on an RGB-D camera, LiDAR, or a preset coupler structure model, and obstacle avoidance constraints for the robotic arm motion planning can be generated in combination with the spatial position of the hook lifting rod. During the path planning process, it can be determined whether the distance between the end effector of the robotic arm and each joint of the robotic arm and the vehicle body structure and coupler area structure is less than a preset safety distance.

[0057] In a specific example, a safe distance of not less than 5 cm between the vehicle train and the surrounding structures of the hook-lifting rod can be used as a reference for obstacle avoidance constraints.

[0058] If the planned path meets the safety distance requirements, the end effector of the robotic arm moves from the pre-aligned position to the execution position according to the planned path; if the distance between the planned path and the vehicle body structure or the coupler area structure is less than the preset safety distance, the motion path is replanned, or the uncoupling action is paused if a safe path cannot be obtained. By introducing obstacle avoidance constraints in the end-effector stage, the risk of collision between the robotic arm and the vehicle body or coupler area structure can be reduced, and the operational safety during dynamic uncoupling can be improved.

[0059] like Figure 2 As shown, this application also provides a collaborative following positioning and end-effector control system 500 for a hump unhooking robot, including a hump unhooking robot 503, an RGB-D camera 502, and a controller 501. The hump unhooking robot 503 includes a wheeled mobile chassis 5031 and a robotic arm end effector 5032 mounted on the wheeled mobile chassis 5031. The controller 501 is communicatively connected to the wheeled mobile chassis 5031, the RGB-D camera 502, and the robotic arm end effector 5032.

[0060] The wheeled mobile chassis 5031 or the controller 501 can be connected to at least one of a wheel encoder, an inertial measurement unit, and a lidar to acquire chassis motion information, vehicle-to-vehicle relative pose information, or trackside path relative pose information.

[0061] The controller 501 includes a job information acquisition module 504, a dynamic tracking estimation module 505, a hook lifting rod visual recognition module 506, a spatial positioning conversion module 507, an execution condition judgment module 508, and a hook removal execution control module 509.

[0062] The operation information acquisition module 504 is used to acquire the target uncoupling position and the corresponding uncoupling operation sequence in the train to be uncoupled.

[0063] The dynamic following estimation module 505 is used to control the wheeled mobile chassis 5031 to travel along the trackside path based on the target uncoupling position and the uncoupling operation sequence, so that the wheeled mobile chassis 5031 dynamically follows the train to be uncoupled.

[0064] The dynamic following estimation module 505 is also used to determine the longitudinal following error and the lateral following error of the wheeled mobile chassis 5031 based on the relative motion information between the wheeled mobile chassis 5031 and the train to be uncoupled and the trackside reference path during the dynamic following process, and to perform state estimation on the longitudinal following error and the lateral following error based on the extended Kalman filter to obtain the following error estimation result.

[0065] The RGB-D camera 502 is used to acquire images and depth data of the coupler area of ​​the train to be uncoupled.

[0066] The hook lifting rod visual recognition module 506 is used to input the image of the hook area into the improved YOLOv5s detection model to identify the image position of key points of the hook lifting rod.

[0067] The spatial positioning conversion module 507 is used to determine the three-dimensional position of the hook lifting rod key point in the camera coordinate system based on the image position of the hook lifting rod key point and the depth value corresponding to the image position in the depth data, and convert the three-dimensional position into the hook lifting rod spatial position in the robot coordinate system according to the calibration relationship between the camera coordinate system and the robot coordinate system.

[0068] The execution condition judgment module 508 is used to determine the target pose of the end effector of the robotic arm based on the spatial position of the hook lifting rod, and to jointly constrain and judge the execution conditions of the target pose of the end effector of the robotic arm based on the following error estimation result and the visual positioning error of the hook lifting rod corresponding to the spatial position of the hook lifting rod.

[0069] The hook removal execution control module 509 is used to control the end effector 5032 of the robotic arm to perform the hook removal action according to the target pose of the end effector of the robotic arm when both the following error estimation result and the visual positioning error of the hook lifting rod meet the preset execution conditions.

[0070] The hook-off execution control module 509 is also used to pause the execution action of the end effector 5032 of the robotic arm when the following error estimation result does not meet the preset execution conditions. The dynamic following estimation module 505 is also used to control the wheeled mobile chassis 5031 to perform following correction.

[0071] The hook removal execution control module 509 is also used to pause the execution action of the end effector 5032 of the robotic arm when the visual positioning error of the hook lifting rod does not meet the preset execution conditions. The hook lifting rod visual recognition module 506 is also used to control the RGB-D camera 502 to re-acquire the image of the hook area and the depth data. The spatial positioning conversion module 507 is also used to update the spatial position of the hook lifting rod.

[0072] Furthermore, the dynamic tracking estimation module 505 performs state estimation on the longitudinal tracking error and the lateral tracking error based on the extended Kalman filter to obtain the tracking error estimation result, including: A following state vector is constructed using the longitudinal following error, the lateral following error, the rate of change of the longitudinal following error, and the rate of change of the lateral following error. The following state vector is predicted based on the acceleration and steering angular velocity of the wheeled mobile chassis to obtain the predicted state; The predicted state is updated based on chassis motion information, vehicle-to-train relative pose information, or trackside path relative pose information collected by at least one of the wheel encoder, inertial measurement unit, and lidar, to obtain the following error estimation result and the covariance information corresponding to the following error estimation result.

[0073] Furthermore, the execution conditions for the target pose at the end of the robotic arm are subject to joint constraint judgment, including: Based on the following error estimation result and the covariance information of the following state vector, the following error range corresponding to when the robotic arm end effector reaches the target pose of the robotic arm end is predicted; If the predicted following error range meets the preset following error threshold and the hook lifting rod visual positioning error meets the preset visual positioning error threshold, then the preset execution condition is determined to be met.

[0074] Each module in the above-mentioned device can be implemented entirely or partially through software, hardware, or a combination thereof. These modules can be embedded in the processor of a computer device in hardware form or independent of it, or stored in the memory of a computer device in software form, so that the processor can call and execute the operations corresponding to each module.

[0075] like Figure 3 As shown, this application also provides a terminal device 400, including a processor 401 and a memory 402. The memory 402 stores a program or instructions that can run on the processor 401. When the program or instructions are executed by the processor 401, they implement the various steps of the above method embodiments and can achieve the same technical effect. To avoid repetition, they will not be described again here.

[0076] This application also provides a readable storage medium storing a program or instructions that, when executed by a processor, implement the various processes of the above-described method embodiments and achieve the same technical effects. To avoid repetition, these will not be described again here.

[0077] The processor can be the processor in the aforementioned system. Readable storage media include computer-readable storage media, such as computer read-only memory, random access memory, magnetic disks, or optical disks.

[0078] It should be noted that, in this document, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes that element. Furthermore, it should be noted that the scope of the methods and apparatuses in the embodiments of this application is not limited to performing functions in the order shown or discussed, but may also include performing functions substantially simultaneously or in the reverse order, depending on the functions involved. For example, the described methods may be performed in a different order than described, and various steps may be added, omitted, or combined. Additionally, features described with reference to certain examples may be combined in other examples.

[0079] Through the above description of the embodiments, those skilled in the art can clearly understand that the methods of the above embodiments can be implemented by means of software plus necessary general-purpose hardware platforms. Of course, they can also be implemented by hardware, but in many cases the former is a better implementation method. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, can be embodied in the form of a computer software product, which is stored in a storage medium (such as a read-only memory). The device includes a number of instructions in a ROM (random access memory), RAM (magnetic disk), or optical disk to cause a terminal to execute the methods described in the various embodiments of this application.

[0080] The embodiments of this application have been described above with reference to the accompanying drawings. However, this application is not limited to the specific embodiments described above. The specific embodiments described above are merely illustrative and not restrictive. Those skilled in the art can make many other forms under the guidance of this application without departing from the spirit and scope of the claims, and all of these forms are within the protection scope of this application.

Claims

1. A method for cooperative following, positioning, and end-effector control of a camel-hump unhooking robot, characterized in that, include: The target uncoupling position and corresponding uncoupling operation sequence of the train to be uncoupled are obtained. Based on the target uncoupling position and the uncoupling operation sequence, the hump uncoupling robot is controlled to travel along the trackside path, so that the wheeled mobile chassis of the hump uncoupling robot dynamically follows the train to be uncoupled. During the dynamic following process, based on the relative motion information between the wheeled mobile chassis and the train to be uncoupled, the longitudinal following error and the lateral following error of the wheeled mobile chassis are determined, and the longitudinal following error and the lateral following error are estimated based on the extended Kalman filter to obtain the following error estimation result. The image and depth data of the coupler area of ​​the train to be uncoupled are acquired by an RGB-D camera, and the image of the coupler area is input into an improved YOLOv5s detection model to identify the image position of the key points of the coupler lifting rod. Based on the image position of the hook lifting rod key point and the depth value corresponding to the image position in the depth data, the three-dimensional position of the hook lifting rod key point in the camera coordinate system is determined, and based on the calibration relationship between the camera coordinate system and the robot coordinate system, the three-dimensional position is converted into the hook lifting rod spatial position in the robot coordinate system. The target pose of the robotic arm end effector is determined based on the spatial position of the hook lifting rod, and the execution conditions of the target pose of the robotic arm end effector are jointly constrained and judged based on the following error estimation result and the visual positioning error of the hook lifting rod corresponding to the spatial position of the hook lifting rod. If both the following error estimation result and the hook lifting rod visual positioning error meet the preset execution conditions, the end effector of the robotic arm of the camel hump unhooking robot is controlled to perform the unhooking action according to the target pose of the end effector of the robotic arm; if at least one of the following error estimation result and the hook lifting rod visual positioning error does not meet the preset execution conditions, the execution action of the end effector of the robotic arm is paused.

2. The method for cooperative following, positioning, and end-effector control of a camel hump unhooking robot according to claim 1, characterized in that, The state estimation of the longitudinal and lateral following errors based on extended Kalman filtering, to obtain the following following error estimation results, includes: A following state vector is constructed using the longitudinal following error, the lateral following error, the rate of change of the longitudinal following error, and the rate of change of the lateral following error. The following state vector is predicted based on the acceleration and steering angular velocity of the wheeled mobile chassis to obtain the predicted state; The predicted state is updated based on chassis motion information, vehicle-to-train relative pose information, or trackside path relative pose information collected by at least one of the wheel encoder, inertial measurement unit, and lidar, to obtain the following error estimation result and the covariance information corresponding to the following error estimation result.

3. The method for cooperative following, positioning, and end-effector control of a camel hump unhooking robot according to claim 2, characterized in that, The joint constraint judgment on the execution conditions of the target pose at the end of the robotic arm includes: Based on the following error estimation result and the corresponding covariance information, predict the following error range when the robotic arm end effector reaches the target pose of the robotic arm end; If the predicted following error range meets the preset following error threshold and the hook lifting rod visual positioning error meets the preset visual positioning error threshold, then the preset execution condition is determined to be met.

4. The method for cooperative following, positioning, and end-effector control of a camel hump unhooking robot according to claim 1, characterized in that, The improved YOLOv5s detection model includes a key point detection branch, which is used to output the image positions of the pivot center and the handle end of the hook rod; The training loss of the improved YOLOv5s detection model includes target classification loss, bounding box regression loss, and key point location loss, enabling the improved YOLOv5s detection model to identify the area where the hook rod is located while determining the hook rod key points used for end-effector alignment.

5. The method for cooperative following, positioning, and end-effector control of a camel hump unhooking robot according to claim 1, characterized in that, The step of determining the three-dimensional position of the hook lifting rod key point in the camera coordinate system based on the image position of the key point and the depth value corresponding to the image position in the depth data includes: Align the image of the coupler region with the depth data pixel by pixel; Filter valid depth values ​​within the neighborhood of the image location of the key point of the hook lifting rod; If the depth value corresponding to the image location is missing or fluctuates beyond a preset range, the effective depth value is filtered and fused by combining depth data from multiple consecutive frames to obtain a corrected depth value. The effective depth value or the corrected depth value obtained through filtering is used as the target depth value; based on the image position of the hook lifting rod key point, the target depth value, and the intrinsic parameters of the RGB-D camera, the three-dimensional position of the hook lifting rod key point in the camera coordinate system is determined.

6. The method for cooperative following, positioning, and end-effector control of a camel hump unhooking robot according to claim 1, characterized in that, The visual positioning error of the hook lifting rod is determined based on the image detection error, depth measurement error, and calibration error. Wherein, the image detection error is the spatial error corresponding to the position error of the hook lifting rod key point image output by the improved YOLOv5s detection model, the depth measurement error is the depth error of the RGB-D camera at the hook lifting rod key point, and the calibration error is the calibration error between the camera coordinate system and the robot coordinate system.

7. The method for cooperative following, positioning, and end-effector control of a hump-unhooking robot according to claim 1, characterized in that, The target pose at the end of the robotic arm includes a pre-alignment pose located outside the spatial position of the hook lifting rod and an execution pose for acting on the hook lifting rod. Controlling the end effector of the robotic arm to perform a hook-unhooking action according to the target pose of the robotic arm end effector includes: Control the end effector of the robotic arm to move to the pre-aligned position; If the following error estimation result and the visual positioning error of the hook rod meet the preset execution conditions for a preset duration, the end effector of the robotic arm is controlled to move from the pre-aligned position to the execution position and perform the hook removal action.

8. The method for cooperative following, positioning, and end-effector control of a camel hump unhooking robot according to claim 7, characterized in that, During the process of controlling the end effector of the robotic arm to move from the pre-aligned position to the execution position, obstacle avoidance constraints are generated based on the car body structure of the train to be uncoupled, the coupler area structure, and the spatial position of the hook lifting rod. When the distance between the planned path and the vehicle body structure or the coupler area structure is less than the preset safety distance, the motion path of the end effector of the robotic arm is replanned or the unhooking action is paused.

9. A collaborative following positioning and end-effector control system for a camel hump unhooking robot, characterized in that, include: The system includes a hump unhooking robot, an RGB-D camera, and a controller. The hump unhooking robot comprises a wheeled mobile chassis and a robotic arm end effector mounted on the wheeled mobile chassis. The controller is communicatively connected to the wheeled mobile chassis, the RGB-D camera, and the robotic arm end effector. The controller is used to obtain the target uncoupling position and the corresponding uncoupling operation sequence in the train to be uncoupled, and based on the target uncoupling position and the uncoupling operation sequence, control the wheeled mobile chassis to travel along the trackside path so that the wheeled mobile chassis dynamically follows the train to be uncoupled. The controller is also used to determine the longitudinal and lateral following errors of the wheeled mobile chassis based on the relative motion information between the wheeled mobile chassis and the train to be uncoupled and the trackside reference path during the dynamic following process, and to perform state estimation on the longitudinal and lateral following errors based on extended Kalman filtering to obtain the following error estimation result. The RGB-D camera is used to acquire images and depth data of the coupler area of ​​the train to be uncoupled. The controller is also used to input the coupler area image into the improved YOLOv5s detection model to identify the image position of the key points of the coupler lifting rod. The controller is also used to determine the three-dimensional position of the hook lifting rod key point in the camera coordinate system based on the image position of the hook lifting rod key point and the depth value corresponding to the image position in the depth data, and to convert the three-dimensional position into the hook lifting rod spatial position in the robot coordinate system according to the calibration relationship between the camera coordinate system and the robot coordinate system. The controller is also used to determine the target pose of the end effector of the robotic arm based on the spatial position of the hook lifting rod, and to jointly constrain and judge the execution conditions of the target pose of the end effector of the robotic arm based on the following error estimation result and the visual positioning error of the hook lifting rod corresponding to the spatial position of the hook lifting rod. The controller is further configured to, when both the following error estimation result and the visual positioning error of the hook lifting rod meet the preset execution conditions, control the end effector of the robotic arm to perform the hook removal action according to the target pose of the end effector of the robotic arm; when the following error estimation result does not meet the preset execution conditions, pause the execution action of the end effector of the robotic arm and control the wheeled mobile chassis to perform following correction; when the visual positioning error of the hook lifting rod does not meet the preset execution conditions, pause the execution action of the end effector of the robotic arm and control the RGB-D camera to re-acquire the image of the hook area and the depth data to update the spatial position of the hook lifting rod.

10. A terminal device, characterized in that, It includes a processor and a memory, wherein the memory stores a computer program that, when executed by the processor, implements the method according to any one of claims 1 to 8.

Images