一种编组站拉风钩机器人系统及控制方法与流程

本发明涉及轨道交通，尤其涉及一种编组站拉风钩机器人系统及控制方法。

背景技术：

1、编组站到达场是货物列车解体作业的重要作业区域。货物列车进入到达场后，需要对车辆制动系统进行排风处理，其中拉风作业通常需要沿车列逐辆寻找车辆缓解阀拉杆或拉风钩，并通过拉动缓解阀拉杆释放副风缸内的压缩空气。传统拉风作业主要依靠人工完成，作业人员需要沿较长车列反复行走、弯腰并实施拉动作业，劳动强度大，且作业过程容易受到高温、低温、雨雪、夜间低照度、站场噪声以及车辆动态运行等因素影响，存在较大的劳动安全风险。因此，利用机器人替代人工完成编组站拉风作业已成为提高站场自动化水平的重要技术方向。

2、现有技术已经针对编组站车辆摘管、摘钩及拉风作业提出了多种机器人技术方案：

3、cn113146628b公开了一种适用于编组站的制动软管摘结机器人系统，包括机器人、信息获取模块、通信控制模块、关键作业目标识别模块、路径规划模块、运动控制模块以及视频监控模块，利用激光雷达与双目视觉识别制动软管摘结目标，并根据目标识别结果规划机械臂运动路径；同时融合差分北斗定位、imu及激光雷达点云等信息实现机器人定位和走行路径规划。

4、cn113894765a公开了一种火车自动摘钩机器人及系统，机器人具有自主移动平台、多自由度机械臂及立体视觉模块，其中移动平台可以配置激光雷达、摄像头、gps和惯性传感器等环境感知设备，以识别和定位待摘车钩并控制机械臂实施自动摘钩。

5、cn121849203a公开了一种列车自动拉风杆操作装置，包括力传感器、rgbd相机、环形麦克风阵列、惯性测量单元、挂钩及信号采集处理板等；其rgbd相机用于获得拉风杆的实时三维图像及位置信息并引导挂钩完成对接，rgbd相机数据与imu数据融合以辅助和修正挂钩的对接路径；力传感器用于作业过程中的力控及安全保护，环形麦克风阵列用于采集拉风过程中的声音并根据预设声学特征判断拉风作业是否成功。

6、综合上述现有技术可以看出，现有技术仍至少存在以下不足：

7、1）定位精度不足，且缺乏定位可信度与机器人停车位置之间的闭环：

8、编组站具有股道数量多、轨道长距离平行、车体结构重复、车辆遮挡明显以及卫星信号易受影响等特点，机器人在长距离自主移动过程中容易受到局部环境特征退化、传感器观测质量变化以及外部定位信号波动等因素影响。现有定位方案虽然能够采用视觉、激光雷达、惯性测量、卫星定位等多种信息，但在复杂站场环境下仍存在定位精度波动、局部定位退化的问题，难以持续为后续精细作业提供稳定、可信的位置基础。现有技术通常在完成目标检测或定位后直接控制机械臂进行作业，缺少定位可信度、位姿质量的评价机制，也缺少根据评价结果进行自主调整的方式。

9、2）拉风钩的识别和精确定位能力仍难以满足机械臂可靠作业要求：

10、现有技术已经能够通过普通相机、深度相机、双目视觉等方式识别车辆缓解阀、拉风杆、挂钩等目标，并获得目标位置或一定的方向信息。但实际车辆车型多样，停车位置存在偏差，拉风钩自身尺寸较小，且其周围常伴随车体构件、管路及其他相似结构，同时还可能受到遮挡、光照变化和视角变化影响。现有目标识别与定位方法在复杂情况下容易出现定位误差、姿态判断不准确或识别结果不稳定，难以持续满足机械臂精确接近、挂接和拉动的要求。

11、3）对拉风作业过程和作业结果的检测仍不够全面可靠：

12、现有技术已经能够利用力传感器监测机械执行过程中的受力状态，并利用麦克风或声音识别方法判断排风状态。但是，拉风作业包含机械挂接、拉动及气体释放等多个相互关联的过程，实际作业中还可能出现机械卡滞、末端滑脱、车辆无风、排风不充分以及非目标声源干扰等不同情况。现有技术对于机械执行状态与排风过程状态之间的关联分析仍不充分，对拉风动作是否有效、排风过程是否正常以及不同异常状态的区分能力仍有提高空间。

13、4）感知、执行、检测及异常处理之间的协同和闭环程度不足：

14、现有机器人系统通常已经具备定位、目标识别、机械臂执行以及状态检测等多个功能模块，但各模块之间往往侧重于顺序执行或局部信息反馈。当目标定位精度不足、位姿质量不高、机械臂挂接异常、拉动失败或排风状态异常时，系统对异常原因的进一步分析以及后续动作调整能力有限，难以根据实际作业状态及时调整机器人位置、目标识别结果、机械臂作业参数及再次执行策略。因此，现有技术在多环节信息相互校验、作业状态反馈以及异常后的自主恢复方面缺乏有效手段。

15、因此，需要进一步提高编组站拉风机器人在复杂环境中的定位可靠性、拉风钩识别与定位精度、作业过程与结果检测能力，并增强机器人定位、目标感知、机械执行和作业状态检测之间的协同控制与闭环处理能力，以满足编组站到达场连续、稳定和自主拉风作业的要求。

技术实现思路

1、为解决上述技术问题，本发明提供了一种编组站拉风钩机器人系统，包括：

2、定位与导航单元，用于融合相机、激光雷达、惯性测量单元（imu）、卫星及rtk差分定位数据，实现高精度定位，并确定机器人在站场中的位姿及定位质量状态；

3、移动单元，与所述定位与导航单元连接，用于在到达场区域内自主规划路径，根据所述位姿自主移动至目标车厢对应的候选停车作业区域；

4、视觉雷达检测单元，用于检测风钩候选区域并提取风钩目标三维点云，利用车厢几何约束和风钩几何特征建立并校正初始位姿，通过模板点云与所述目标三维点云配准求解风钩在机器人基坐标系下的六自由度位姿，并评价位姿质量；

5、机械臂执行单元，用于根据所述六自由度位姿执行接近、挂接、预紧、主拉及释放动作；

6、力觉检测单元，设置于机械臂执行单元的末端，用于采集机械臂末端受力信息，根据末端力信号和末端位移识别有效拉动事件以及滑脱、卡滞、过载及偏碰状态；

7、声阵列检测单元，用于采集多通道声音信号，并在所述有效拉动事件形成后检测目标风钩区域的排风开始、排风保持和排风衰减完成状态；

8、控制单元，分别与所述定位与导航单元、移动单元、视觉雷达检测单元、机械臂执行单元、力觉检测单元及声阵列检测单元连接，用于根据所述六自由度位姿和车型参数库优化机械臂作业轨迹及力控参数，根据所述末端受力信息修正机械臂末端沿拉风方向的运动参数，并根据拉动状态、排风状态判断本节车厢拉风钩操作是否完成；在作业异常且满足重新感知条件时协调所述视觉雷达检测单元重新检测拉风钩，基于重新求解的拉风钩六自由度位姿更新所述机械臂作业轨迹及力控参数；

9、通信单元，与所述控制单元连接，用于接收远程指挥中心下发的作业任务并上传机器人位置、定位质量、作业状态及异常信息。

10、所述定位与导航单元包括相机、激光雷达、imu、卫星定位装置、rtk差分定位装置及融合定位模块；所述融合定位模块具体用于：利用激光雷达采集的点云数据与imu采集的姿态数据进行slam建图，生成环境地图；将相机采集的环境图像数据与激光雷达采集的点云数据进行配准融合，补充环境感知的语义信息；将slam输出的位姿数据、卫星定位装置接收的卫星定位数据进行融合，在卫星信号受限时维持定位精度；将rtk差分定位装置接收的rtk差分校正数据与卫星定位信号进行融合，在rtk处于固定解时，将定位精度提升至厘米级；将以上多源融合结果进行联合优化，输出最终定位结果及环境地图，执行机器人导航任务；同时还输出机器人连续位姿、位姿不确定度及定位质量状态。

11、所述移动单元包括：自由导航模块，用于根据作业任务及定位与导航单元输出的定位结果，自主规划从当前位置到目标作业位置的行走路径，当所述定位质量状态低于预设定位质量阈值时，控制所述移动单元降速、停止或者禁止进入机械臂作业阶段；避障模块，用于在行走路径上检测到障碍物时，重新规划避障路径；自主充电模块，用于在作业任务完成后，根据定位结果自主导航返回充电桩。

12、所述视觉雷达检测单元利用目标检测模型或实例分割模型获得所述拉风钩候选区域；将激光雷达点云投影至图像平面，并选择落入所述候选区域且满足深度范围约束的点形成所述拉风钩目标三维点云；从所述拉风钩目标三维点云及其邻域点云中确定车厢侧壁平面和轨道延伸方向，并根据拉风钩目标三维点云的质心、主方向、尺寸、包围盒、几何关键点确定所述初始六自由度位姿；根据当前车型从模板库中调取对应的拉风钩模板点云，并以经几何约束校正后的初始六自由度位姿作为gicp点云配准初值；根据所述点云精配准结果确定所述六自由度位姿的质量指标；所述质量指标包括配准均方根误差、有效对应点比例、拉风钩相对于车体的几何约束残差和位姿协方差；当所述质量指标不满足预设质量条件时，根据当前目标观测方向、机械臂可达空间和当前机器人停车位姿确定新的候选停车位姿，并在移动至所述新的候选停车位姿后重新获取拉风钩目标三维点云并重新进行六自由度位姿求解。车型参数库中存储不同车型对应的标准拉风钩位姿、机械臂基准挂接姿态和基准作业轨迹。

13、所述控制单元计算当前拉风钩六自由度位姿相对于所述标准拉风钩位姿的平移偏差和旋转偏差，并基于所述平移偏差和旋转偏差修正机器人候选停车位姿、机械臂挂接姿态以及机械臂末端轨迹；以机械臂末端位姿误差、关节运动量及轨迹平滑性为优化目标，以机械臂可达空间、关节运动限制、碰撞安全距离和允许出力范围为约束，求解所述机械臂作业轨迹及力控参数。

14、所述力觉检测单元将机械臂末端力和力矩信号转换至以拉风方向为主轴的拉风作业坐标系，获得拉风方向力、侧向力、法向力及拉风方向力变化率，并结合机械臂末端位移依次识别接触、挂接、预紧和主拉状态；机械臂在挂接阶段通过末端受力确认拉风钩与末端执行器的作用关系，在预紧阶段建立预设预紧力，并在主拉阶段根据拉风方向力反馈修正机械臂末端沿拉风方向的位移、速度及出力参数；当末端拉动位移达到最小有效位移阈值，且拉风方向力达到有效拉动阈值或拉风方向力变化率达到机构动作阈值，并且侧向力、法向力未超过对应安全阈值时，判定形成有效拉动事件并记录有效拉动事件时刻；当拉风方向力持续升高而末端位移未达到最小有效位移阈值时，判定为风钩卡滞并停止继续增大拉力；当完成挂接后拉风方向力快速下降至滑脱阈值以下时，判定为末端滑脱并执行回退及重新挂接；当拉风方向力、侧向力或法向力超过安全阈值时判定为过载或偏碰；当检测到风钩卡滞、末端滑脱、过载或偏碰时，向控制单元发送异常状态及对应的力觉失败类型，以使控制单元执行停止、回退、重新定位或人工介入。

15、所述声阵列检测单元在所述有效拉动事件形成后，以有效拉动事件时刻为时间基准建立排风声检测时间窗，在所述检测时间窗内提取多通道声音信号的短时能量、频带能量、频谱特征、声源方向、排风持续时间和能量衰减特征；当声音事件位于所述检测时间窗内、声源方向对应目标风钩区域且声学特征满足排风开始条件时，判定目标排风开始；当排风声音持续达到预设持续条件并衰减至结束阈值以下且保持预设安静时间时，判定排风完成。

16、所述控制单元还用于按照接近、接触确认、挂接确认、预紧、主拉、排风开始确认、排风保持、排风完成确认及释放复位的顺序控制机械臂执行单元、力觉检测单元和声阵列检测单元；在力觉检测单元确认有效拉动事件后启动所述排风声检测时间窗，使声音判定在时序上位于有效拉动事件之后；当有效拉动事件成立、目标方向的排风声在所述检测时间窗内出现、排风持续条件和排风衰减完成条件均满足，且不存在卡滞、滑脱、过载或偏碰时，判断本节车厢拉风钩操作完成；当有效拉动事件成立但未出现目标方向排风声时输出拉动未排风状态；当已检测到目标方向排风声但持续时间或衰减完成条件不满足时输出排风不充分状态；；当检测到风钩卡滞、末端滑脱、过载、偏碰或非目标方向声源时，向控制单元发送异常状态及对应的失败类型；控制单元根据不同异常状态选择保持当前姿态、再次拉动、回退重新挂接、重新获取拉风钩六自由度位姿或者停止作业中的至少一种处理方式，其中，在目标位姿或挂接状态不满足预设条件时，先重新求解拉风钩六自由度位姿，再更新机械臂作业参数。

17、本发明还提供了一种利用所述机器人系统实施的控制方法，包括以下步骤：

18、s1：接收远程指挥中心下发的作业任务，启动机器人系统；

19、s2：利用定位与导航单元进行高精度定位、确定机器人在站场中的位姿及定位质量状态，并利用移动单元自主规划路径移动至作业区域；

20、s3：检测拉风钩候选区域并提取拉风钩目标三维点云，基于车厢侧壁、轨道延伸方向及拉风钩几何特征建立并校正拉风钩初始六自由度位姿，以校正后的初始六自由度位姿作为点云精配准初值，求解拉风钩六自由度位姿并评价位姿质量；

21、s4：当位姿质量不满足预设条件时，重新获取目标数据或者调整机器人候选停车位姿后重新求解所述拉风钩六自由度位姿；当位姿质量满足预设条件时，根据所述拉风钩六自由度位姿和车型参数库确定机械臂作业模板，在机械臂可达空间、碰撞安全距离和允许出力范围的约束下确定机械臂作业轨迹及力控参数；

22、s5：控制机械臂依次执行接触、挂接、预紧和主拉动作，利用力觉检测单元识别有效拉动事件及异常力状态，并同步采集声音信号；

23、s6：在确认有效拉动事件后建立排风声检测时间窗，依次判断目标方向排风声是否开始、是否持续以及是否衰减完成；

24、s7：当有效拉动、目标排风开始、排风持续和排风衰减完成条件均满足且不存在异常力状态时，判断本节车厢拉风钩操作完成并转入下一节车厢；

25、s8：当出现拉动未排风、排风不充分、末端滑脱、风钩卡滞、过载、偏碰或声源方向错误时，根据失败类型执行保持、二次拉动、回退或停止；在允许自动重试且未达到重试上限时返回步骤s3重新检测风钩位姿并更新作业参数；

26、s9：重复步骤s3至s8，直至完成全部作业任务，控制移动单元自主返回充电桩。

27、进一步的，步骤s2具体包括：利用相机采集环境图像数据，利用激光雷达采集环境点云数据；利用imu采集机器人姿态数据，利用卫星定位装置接收多源卫星定位信号，利用rtk差分定位装置接收rtk差分校正数据；将上述多源数据进行融合处理，输出高精度定位结果及环境地图；根据作业任务及定位结果，自主规划从当前位置到目标作业位置的行走路径，在行走过程中利用避障模块检测路径上的障碍物，当检测到障碍物时重新规划避障路径。

28、进一步的，步骤s3-s5具体包括：根据风钩六自由度位姿、车型匹配结果和机械臂当前状态，从车型参数库中选择相应的基准作业模板；计算风钩六自由度位姿相对于标准作业位姿的平移偏差和旋转偏差，对候选停车位姿、挂接姿态和机械臂末端轨迹进行补偿；以末端位姿误差、关节运动量和轨迹平滑性为优化目标，以机械臂可达空间、关节运动限制、碰撞安全距离和预设出力范围为约束，求解机械臂作业轨迹及力控参数；在执行过程中，根据机械臂末端力反馈修正末端沿拉风方向的位移和速度。

29、与现有技术相比，本发明具有以下有益效果：

30、1）提高复杂编组站环境下机器人的定位稳定性和可信度：

31、本发明融合视觉、激光雷达、imu、卫星定位及rtk差分定位等多源信息，并针对编组站轨道长距离平行、车体结构重复以及卫星定位状态变化等特点，对不同传感器观测的有效性和定位退化状态进行判断和调整，从而降低单一传感器失效、局部环境几何退化以及异常全局定位观测对机器人连续定位的影响。同时输出定位质量状态，并将定位质量与机器人移动及后续机械臂作业条件关联，使机器人不仅能够获得定位结果，还能够根据定位结果的可信程度调整运行状态，从而提高复杂站场中的自主导航和精细作业可靠性。

32、2）提高拉风钩识别及空间位姿确定的准确性，为机械臂可靠挂接提供更精确的目标信息：

33、本发明首先利用视觉与点云信息检测并提取拉风钩目标三维数据，再结合车厢结构特征和拉风钩自身几何特征对目标空间姿态进行确定和校正，并进一步利用目标实测点云与模板点云进行精确配准，获得拉风钩的六自由度位姿。通过由粗到精的目标检测与位姿求解方式，能够降低车辆停车偏差、局部遮挡、车体背景干扰以及目标姿态变化对定位结果的影响，使机械臂能够依据拉风钩实际空间位置和方向执行接近、挂接和拉动操作，从而提高目标定位精度和机械臂挂接成功率。

34、3）提高拉风作业过程及作业结果检测的准确性和可靠性：

35、本发明利用机械臂末端力觉信息对接触、挂接、预紧和主拉等不同作业阶段的受力状态进行检测，并结合机械臂运动状态识别滑脱、卡滞、过载、偏碰等异常情况；同时利用声阵列采集拉风作业过程中的声音信息，对排风声及其声源方向、持续状态和衰减状态进行分析。通过将机械执行过程中的状态信息与排风过程中的状态信息按照拉风作业的实际过程进行关联，提高了作业过程状态识别、排风状态确认以及二者在整个作业流程中的协同和耦合程度，并能够对不同类型的作业异常进行区分，从而提高复杂站场环境下拉风作业状态检测和作业结果判断的准确性与可靠性。

36、4）实现机器人定位、目标识别、机械臂执行和作业结果检测之间的闭环耦合，提高系统自主作业和异常恢复能力：

37、本发明不是将定位、识别、机械臂控制和作业检测作为彼此独立的功能模块，而是使各阶段的输出结果参与后续作业决策：机器人定位结果为目标感知和机械臂作业提供空间基础；拉风钩识别结果及空间位姿用于确定机器人作业位置和机械臂作业参数；机械臂执行结果和作业状态检测结果反馈至控制单元，并参与后续作业策略的确定；当出现定位质量下降、目标识别或定位异常、机械臂执行异常以及拉风作业结果异常时，控制系统根据异常发生的环节及状态调整相应的感知、定位或执行过程。由此形成从“机器人定位—拉风钩感知—机械臂执行—作业状态检测—异常反馈与再执行”的闭环控制，提高机器人面对不同车型、停车偏差和复杂现场工况时的自主适应能力、连续作业能力以及整体作业成功率。