[发明专利]一种井下轨道车辆自动驾驶机器人及其控制方法

权利要求书

1.一种井下轨道车辆自动驾驶机器人，包括感知定位系统（1）、控制系统（2）、执行系统（3）和驱动系统（4）；

其特征在于，执行系统（3）包括躯干、头部、机械腿（301）和两个机械臂（303）；头部和机械腿（301）分别设置在躯干的顶部和底部前面，两个机械臂（303）分别设置在躯干左右两侧面的上部，机械腿（301）、两个机械臂（303）分别与第三伺服电机（307）、第一伺服电机（305）、第二伺服电机（304）连接；

感知定位系统（1）包括第一毫米波雷达（102）、第二毫米波雷达（103）、惯导系统（105）、第一双目相机（101）、第二双目相机（104）、第三双目相机和5G定位标签（107）；第一双目相机（101）、第二双目相机（104）、第三双目相机分别设置在头部的前面和左右两个侧面，第一毫米波雷达（102）、第二毫米波雷达（103）分别设置在躯干顶面左右两边缘处，5G定位标签（107）和惯导系统（105）均设置在躯干内；

控制系统（2）包括上位机（201）、VCU整车控制器（202）、下位机（203）和车载通讯控制器（204），上位机（201）、VCU整车控制器（202）、下位机（203）和车载通讯控制器（204）均设置在躯干内，上位机（201）分别与惯导系统（105）、第一双目相机（101）、第二双目相机（104）、第三双目相机、第一毫米波雷达（102）、第二毫米波雷达（103）电连接，上位机（201）与VCU整车控制器（202）、下位机（203）和车载通讯控制器（204）电连接，下位机（203）与第三伺服电机（307）、第一伺服电机（305）、第二伺服电机（304）连接；

驱动系统（4）包括电池配电箱（401）和蓄电池（402），蓄电池（402）与电池配电箱（401）连接，电池配电箱（401）与感知定位系统（1）、控制系统（2）和执行系统（3）电连接。

2.根据权利要求1所述的一种井下轨道车辆自动驾驶机器人，其特征在于，所述机械腿（301）、两个机械臂（303）与躯干可拆卸连接。

3.根据权利要求1所述的一种井下轨道车辆自动驾驶机器人，其特征在于，还包括第四伺服电机（306），第四伺服电机（306）设置在躯干内与头部连接，下位机（203）与第四伺服电机（306）连接。

4.根据权利要求1所述的一种井下轨道车辆自动驾驶机器人，其特征在于，所述躯干前面中部设有第四双目相机（108），第四双目相机（108）与上位机（201）电连接。

5.根据权利要求3所述的一种井下轨道车辆自动驾驶机器人，其特征在于，所述惯导系统（105）与第三伺服电机（307）、第一伺服电机（305）、第二伺服电机（304）和第四伺服电机（306）连接。

6.根据权利要求1所述的一种井下轨道车辆自动驾驶机器人，其特征在于，所述躯干上设有RFID射频标签（106）。

7.根据权利要求1-6所述的一种井下轨道车辆自动驾驶机器人控制方法，其特征在于，包括以下步骤：

步骤一：启停控制：机器人的第二双目相机（104）和第三双目相机扫描装货地点的识别标签，将到达指定地点的信息传递给机器人，上位机（201）将处理后的数据传递给VCU整车控制器（202），VCU整车控制器（202）下达指令给下位机（203）控制机械臂（303）按下启停/牵引按钮，完成轨道车辆停车动作；待货物装载完成后，装货地点通知VCU整车控制器（202），VCU整车控制器（202）下达指令给下位机（203）控制机械臂（303）按下启停/牵引按钮，完成轨道车辆启动；机器人的第二双目相机（104）和第三双目相机扫描装货地点的识别标签，将到达指定地点的信息传递给机器人，上位机（201）将处理后的数据传递给VCU整车控制器（202），VCU整车控制器（202）下达指令给下位机（203）控制机械臂（303）按下启停/牵引按钮，完成轨道车辆停车动作；

步骤二：行驶控制：当轨道车辆运行时，第一双目相机（101）实时监测车辆前方障碍物信息，第二双目相机（104）、第三双目相机实时检测车辆两侧的障碍物信息，第一毫米波雷达（102）和第二毫米波雷达（103）实时检测车辆两侧有无人员或车辆入侵，检测结果传输至上位机（201）进行感知融合，上位机（201）接收这些信号并进行决策融合处理，处理结果为：当前方无障碍物且无弯道时，上位机（201）输出信号为0000；当前方有障碍物，但大于设定的安全距离时，输出信号为0001；当前方有障碍物，小于设定的安全距离但大于设定的危险距离时，输出信号为0101；当前方有障碍物，且小于设定的危险距离时，输出信号为0111；处理结果传输到VCU整车控制器（202）；

当上位机（201）输出信号为0000时， VCU整车控制器（202）下达指令给下位机（203），下位机（203）解析VCU整车控制器（202）的指令，控制机械臂（303）推动加速档把，使机械腿（301）维持当前工作状态，加速到轨道车辆规定最高速度，实现轨道车辆直线加速控制；

当输出信号由0000变为0001时，VCU整车控制器（202）下达指令给下位机（203），下位机（203）解析VCU整车控制器（202）的指令，控制机械臂（303）和机械腿（301）维持当前状态，保持轨道车辆原速动作；

当输出信号由0001变为0101时，VCU整车控制器（202）下达指令给下位机（203），下位机（203）解析VCU整车控制器（202）的指令，控制机械臂（303）拉下速度档把，机械腿（301）缓慢踏下刹车踏板，实现轨道车辆减速动作；

当输出信号由0101变为0111时，VCU整车控制器（202）下达指令给下位机（203），下位机（203）解析VCU整车控制器（202）的指令，控制机械腿（301）实现紧急刹车，同时机械臂（303）将速度档把拉至最低，完成轨道车辆紧急制动，实现停车；

等待障碍物被清除，输出信号由0111变为0000时VCU整车控制器（202）下达指令给下位机（203），下位机（203）解析VCU整车控制器（202）的指令，控制一只机械臂（303）按下启动按钮，另一只机械臂（303）推动加速档把，收回机械腿（301），加速到轨道车辆规定最高速度，实现轨道车辆直线加速控制；

步骤三：弯道处理：当第一双目相机（101）检测到前方有弯道时，第一双目相机（101）将感知信息传递给上位机（201）：当轨道车辆前方无弯道且无障碍物时，上位机（201）输出信号为0000；当前方有弯道但无障碍物时，上位机（201）输出信号为1000；当前方有弯道且存在障碍物时，若轨道车辆与障碍物间距离大于安全距离，输出信号为1001，若轨道车辆与障碍物间距离小于安全距离大于危险距离，则输出信号为1101，若距离小于危险距离，则输出信号为1111；决策信号传递至VCU整车控制器（202）；

当上位机（201）输出信号由0000变为1000时，VCU整车控制器（202）下达指令给下位机（203），下位机（203）解析VCU整车控制器（202）的指令，控制机械臂（303）拉下速度档把，机械腿（301）踏下刹车踏板，降低车速至规定转向速度，准备转向；

当输出信号由1000变为1001时，VCU整车控制器（202）下达指令给下位机（203），下位机（203）解析VCU整车控制器（202）的指令，控制机械臂（303）和机械腿（301）保持原状态，维持低车速；

当输出信号由1001变为1101时，VCU整车控制器（202）下达指令给下位机（203），下位机（203）解析VCU整车控制器（202）的指令，控制机械臂（303）拉下速度档把，机械腿（301）踏下刹车踏板，降低至最低车速；

当输出信号由1101变为1111时，VCU整车控制器（202）下达指令给下位机（203），下位机（203）解析VCU整车控制器（202）的指令，控制机械腿（301）实现紧急刹车，同时机械臂（303）将速度档把拉至最低，完成轨道车辆紧急制动，实现停车；

当弯道处障碍物被清除后，上位机（201）输出信号由1111变为1000时，VCU整车控制器（202）下达指令给下位机（203），下位机（203）解析VCU整车控制器（202）的指令，控制一只机械臂（303）按下启动按钮，另一只机械臂（303）推动加速档把，收回机械腿（301），加速到轨道车辆规定转向速度，实现轨道车辆转向控制；

当轨道车辆驶过弯道，上位机（201）输出信号由1000变为0000时，VCU整车控制器（202）下达指令给下位机（203），下位机（203）解析VCU整车控制器（202）的指令，控制机械臂（303）推动加速档把，收回机械腿（301），加速到轨道车辆规定最高速度，实现轨道车辆直线加速控制；

步骤四：充能处理：当第四双目相机（108）观测到轨道车辆的能源不足时，向VCU整车控制器（202）传递信号，VCU整车控制器（202）处理后发出命令控制机器人驾驶轨道车辆前往充能站。