一种井下轨道车辆自动驾驶机器人及其控制方法与流程

1.本发明涉及一种驾驶机器人，具体涉及一种井下轨道车辆自动驾驶机器人及其控制方法。


背景技术：

2.我国矿井辅助运输系统多采用轨道式运输车辆，包括：井下电机车、单轨吊、无极绳绞车、齿轨车等，车辆操作方式目前整体还处于半自动化、半机械化及人工辅助运输的落后现状，其效率以及安全性已经极大限制了矿井的安全高效生产和智能化水平提高。近年来，随着“智慧矿山”建设口号的提出，无人驾驶技术开始被应用到矿山井下有轨运输，其中环境智能感知技术和自动控制技术是实现轨道车辆无人驾驶的重要前提，也是实现无人驾驶轨道车辆安全稳定行驶的重要保障。
3.目前，井下轨道车辆在无人驾驶远程监控、电车驱动能量管理、系统调度等几个方面已有部分研究。例如，专利申请号为201921053940.1，名称为“一种矿用电机车自动刹车装置”发明专利，公开了一种矿用电机车自动刹车装置，这是一种辅助刹车系统，该方案仅设计了一种刹车装置，未提及到无人驾驶相关技术；专利申请号为202010198239.x，名称为“一种自动驾驶机器人”发明专利，公开了一种路面车辆的自动驾驶机器人，该方案对自动驾驶机器人的感知系统和控制系统的描述非常粗略，仅设计了机器人的外观机械形状，对机器人的控制方法未提及，且该机器人是针对路面行驶汽车设计，不适用井下轨道车辆的行驶工况；专利申请号为202110366179.2，名称为“一种无驾驶舱式井下无人驾驶电机车及其控制方法”发明专利，该发明主要对电机车进行无人化改造，适用于新型电机车制造，但对已使用的有人驾驶电机车进行无人驾驶改造成本较高、周期较长。
4.在现有技术水平下，人工驾驶轨道车辆在井下环境恶劣的情况下工作效率低下，安全及可靠性较差，但是对井下轨道车辆进行无人化改造的成本过高、周期较长。为此，本发明提出一种井下轨道车辆自动驾驶机器人，可以为上述问题提供更好的解决方案进而提高矿区劳动效率，降低无人驾驶改造成本。


技术实现要素：

5.针对上述现有技术存在的问题，本发明提供一种井下轨道车辆自动驾驶机器人及其控制方法，无人驾驶轨道车辆应用效果好，同时降低了轨道车辆无人化改造的成本。
6.为了实现上述目的，本发明采用的技术方案是：一种井下轨道车辆自动驾驶机器人，包括，包括感知定位系统、控制系统、执行系统和驱动系统；执行系统包括躯干、头部、机械腿和两个机械臂；头部和机械腿分别设置在躯干的顶部和底部前面，两个机械臂分别设置在躯干左右两侧面的上部，机械腿、两个机械臂分别与第三伺服电机、第一伺服电机、第二伺服电机连接；感知定位系统包括第一毫米波雷达、第二毫米波雷达、惯导系统、第一双目相机、第二双目相机、第三双目相机和5g定位标签；第一双目相机、第二双目相机、第三双目相机分别设置在头部的前面和左右两个侧面，第一毫米波雷达、第二毫米波雷达分别设
置在躯干顶面左右两边缘处，5g定位标签和惯导系统均设置在躯干内；控制系统包括上位机、vcu整车控制器、下位机和车载通讯控制器，上位机、vcu整车控制器、下位机和车载通讯控制器均设置在躯干内，上位机分别与惯导系统、第一双目相机、第二双目相机、第三双目相机、第一毫米波雷达、第二毫米波雷达电连接，上位机与vcu整车控制器、下位机和车载通讯控制器电连接，下位机与第三伺服电机、第一伺服电机、第二伺服电机连接； 驱动系统包括电池配电箱和蓄电池，蓄电池与电池配电箱连接，电池配电箱与感知定位系统、控制系统和执行系统电连接。
7.进一步的，所述机械腿、两个机械臂与躯干可拆卸连接；用于满足各种类型的井下轨道车辆无人驾驶。
8.进一步的，还包括第四伺服电机，第四伺服电机设置在躯干内与头部连接，下位机与第四伺服电机连接。
9.进一步的，所述躯干前面中部设有第四双目相机，第四双目相机与上位机电连接。
10.进一步的，所述惯导系统与第三伺服电机、第一伺服电机、第二伺服电机和第四伺服电机连接。
11.进一步的，所述躯干上设有rfid射频标签；用于对机器人实现定位，防止机器人遗失。
12.一种井下轨道车辆自动驾驶机器人控制方法：步骤一：启停控制：机器人的第二双目相机和第三双目相机扫描装货地点的识别标签，将到达指定地点的信息传递给机器人，上位机将处理后的数据传递给vcu整车控制器，vcu整车控制器下达指令给下位机控制机械臂按下启停/牵引按钮，完成轨道车辆停车动作；待货物装载完成后，装货地点通知vcu整车控制器，vcu整车控制器下达指令给下位机控制机械臂按下启停/牵引按钮，完成轨道车辆启动；机器人的第二双目相机和第三双目相机扫描装货地点的识别标签，将到达指定地点的信息传递给机器人，上位机将处理后的数据传递给vcu整车控制器，vcu整车控制器下达指令给下位机控制机械臂按下启停/牵引按钮，完成轨道车辆停车动作；步骤二：行驶控制：当轨道车辆运行时，第一双目相机实时监测车辆前方障碍物信息，第二双目相机、第三双目相机实时检测车辆两侧的障碍物信息，第一毫米波雷达和第二毫米波雷达实时检测车辆两侧有无人员或车辆入侵，检测结果都通过can总线传输至上位机进行感知融合，上位机接收这些信号并进行决策融合处理，处理结果为：当前方无障碍物且无弯道时，上位机输出信号为0000；当前方有障碍物，但大于设定的安全距离时，输出信号为0001；当前方有障碍物，小于设定的安全距离但大于设定的危险距离时，输出信号为0101；当前方有障碍物，且小于设定的危险距离时，输出信号为0111；处理结果通过can总线传输到vcu整车控制器；当上位机输出信号为0000时，机器人内部的vcu整车控制器下达指令给下位机，下位机解析vcu整车控制器的指令，控制机械臂推动加速档把，使机械腿维持当前工作状态，加速到轨道车辆规定最高速度，实现轨道车辆直线加速控制；当输出信号由0000变为0001时，vcu整车控制器下达指令给下位机，下位机解析vcu整车控制器的指令，控制机械臂和机械腿维持当前状态，保持轨道车辆原速动作；当输出信号由0001变为0101时，vcu整车控制器下达指令给下位机，下位机解析
vcu整车控制器的指令，控制机械臂拉下速度档把，机械腿缓慢踏下刹车踏板，实现轨道车辆减速动作；当输出信号由0101变为0111时，vcu整车控制器下达指令给下位机，下位机（203）解析vcu整车控制器的指令，控制机械腿实现紧急刹车，同时机械臂将速度档把拉至最低，完成轨道车辆紧急制动，实现停车；等待障碍物被清除，输出信号由0111变为0000时vcu整车控制器下达指令给下位机，下位机解析vcu整车控制器的指令，控制一只机械臂按下启动按钮，另一只机械臂推动加速档把，收回机械腿，加速到轨道车辆规定最高速度，实现轨道车辆直线加速控制；步骤三：弯道处理：当第一双目相机检测到前方有弯道时，第一双目相机将感知信息传递给上位机：当轨道车辆前方无弯道且无障碍物时，上位机输出信号为0000；当前方有弯道但无障碍物时，上位机输出信号为1000；当前方有弯道且存在障碍物时，若轨道车辆与障碍物间距离大于安全距离，输出信号为1001，若轨道车辆与障碍物间距离小于安全距离大于危险距离，则输出信号为1101，若距离小于危险距离，则输出信号为1111；决策信号经can总线传递至vcu整车控制器；当上位机输出信号由0000变为1000时，vcu整车控制器下达指令给下位机，下位机解析vcu整车控制器的指令，控制机械臂拉下速度档把，机械腿踏下刹车踏板，降低车速至规定转向速度，准备转向；当输出信号由1000变为1001时，vcu整车控制器下达指令给下位机，下位机解析vcu整车控制器的指令，控制机械臂和机械腿保持原状态，维持低车速；当输出信号由1001变为1101时，vcu整车控制器下达指令给下位机，下位机解析vcu整车控制器的指令，控制机械臂拉下速度档把，机械腿踏下刹车踏板，降低至最低车速；当输出信号由1101变为1111时，vcu整车控制器下达指令给下位机，下位机解析vcu整车控制器的指令，控制机械腿实现紧急刹车，同时机械臂将速度档把拉至最低，完成轨道车辆紧急制动，实现停车；当弯道处障碍物被清除后，上位机输出信号由1111变为1000时，vcu整车控制器下达指令给下位机，下位机解析vcu整车控制器的指令，控制一只机械臂按下启动按钮，另一只机械臂推动加速档把，收回机械腿，加速到轨道车辆规定转向速度，实现轨道车辆转向控制；当轨道车辆驶过弯道，上位机输出信号由1000变为0000时，vcu整车控制器下达指令给下位机，下位机解析vcu整车控制器的指令，控制机械臂推动加速档把，收回机械腿，加速到轨道车辆规定最高速度，实现轨道车辆直线加速控制；步骤四：充能处理：当第四双目相机观测到轨道车辆的能源不足时，向vcu整车控制器传递信号，vcu整车控制器处理后发出命令控制机器人驾驶轨道车辆前往充能站。
13.与现有技术相比，本发明双目相机结合毫米波雷达，可以做到分辨障碍物类型及距离，通过自动驾驶机器人替代驾驶员，对于轨道车辆的改动极少，便于改造，且可以回收利用，普适性高，节约制造成本；本发明能够自主采集，分析处理，并将处理结果应用到后续决策层，控制层，执行层，整个过程无需人员参与，节约了井下轨道车辆的人员成本，提高了工作效率；采用5g传输，传输和控制速度更快；使用5g uwb定位，定位精度更高且可靠；本发明将轨道车辆无人驾驶与遥控相结合，遇到紧急情况交予地面人员遥控，更加安全；本发明
还采用rfid射频标签进行定位与识别，从而实现自主装卸货,同时大大提高安全性；本发明将惯导与伺服电机相连，形成闭环控制系统，实现对速度的精确控制。
附图说明
14.图1为本发明主视整体结构示意图；图2为本发明左视整体结构示意图；图3为本发明的控制流程图；图4为本发明的机械臂控制流程图；图5为本发明机器人在轨道车辆中放置示意图；图中：1、感知定位系统，2、控制系统，3、执行系统，4、驱动系统，101、第一双目相机，102、第一毫米波雷达，103、第二毫米波雷达，104、第二双目相机，105、惯导系统，106、rfid电子标签，107、5g定位标签，108、第四双目相机，201、上位机，202、vcu整车控制器，203、下位机，204、车载通讯控制器，301、机械腿，302、机械手，303、机械臂，304、第一伺服电机，305、第二伺服电机，306、第四伺服电机，307、第三伺服电机，401、电池配电箱，402、蓄电池。
具体实施方式
15.下面结合附图对本发明作进一步说明。
16.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
17.如图1和图2所示，本发明提供一种技术方案：包括感知定位系统1、控制系统2、执行系统3和驱动系统4；执行系统3整体呈机器人形状，用于代替驾驶员实现轨道车辆安全驾驶；执行系统3包括躯干、头部、机械腿301和两个机械臂303；头部和机械腿301分别设置在躯干的顶部和底部前面，两个机械臂303分别设置在躯干左右两侧面的上部，机械腿301、两个机械臂303分别与第三伺服电机307、第一伺服电机305、第二伺服电机304连接，伺服电机分别用于控制机械臂303、机械腿301的工作；增设第四伺服电机306，第四伺服电机306设置在躯干内与头部连接，用于控制机器人头部旋转，使双目相机的探测角度范围更加广泛。
18.感知定位系统1包括第一毫米波雷达102、第二毫米波雷达103、惯导系统105、第一双目相机101、第二双目相机104、第三双目相机和5g定位标签107；第一双目相机101、第二双目相机104、第三双目相机分别设置在头部的前面和左右两个侧面，第一毫米波雷达102、第二毫米波雷达103分别设置在躯干顶面左右两边缘处，双目相机和毫米波雷达用于环境感知，5g定位标签107和惯导系统105均设置在躯干内；惯导系统105用于位姿纠正，5g定位标签107用于定位；控制系统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、第四伺服电机306连接；驱动系统4包括电池配电箱401和蓄电池402，蓄电池402与电池配电箱401连接，电池配电箱401与感知定位系统1、控制系统2和执行系统3电连接，为其中各机构提供动力。
19.如图3所示，轨道车辆（电机车）行驶控制方法，如图5所示，将本技术机器人放置在轨道车辆（电机车）中的驾驶位置上，机械臂303和机械腿301设置在相应的驾驶操作机构上用于操作轨道车辆（电机车）。
20.步骤一：启停控制：机器人的第二双目相机104和第三双目相机扫描装货地点的识别标签，将到达指定地点的信息传递给机器人，上位机201将处理后的数据传递给vcu整车控制器202，vcu整车控制器202下达指令给下位机203控制机械臂303按下启停/牵引按钮，完成轨道车辆停车动作，为了更加准确的按下按钮，在操作按钮的机械臂303上设置机械手302；待货物装载完成后，装货地点通知vcu整车控制器202，vcu整车控制器202下达指令给下位机203控制机械手302按下启停/牵引按钮，完成轨道车辆启动；机器人的第二双目相机104和第三双目相机扫描装货地点的识别标签，将到达指定地点的信息传递给机器人，上位机201将处理后的数据传递给vcu整车控制器202，vcu整车控制器202下达指令给下位机203控制机械手302按下启停/牵引按钮，完成轨道车辆停车动作；步骤二：行驶控制：当轨道车辆运行时，第一双目相机101实时监测车辆前方障碍物信息，第二双目相机104、第三双目相机实时检测车辆两侧的障碍物信息，第一毫米波雷达102和第二毫米波雷达103实时检测车辆两侧有无人员或车辆入侵，检测结果都通过can总线传输至上位机201进行感知融合，上位机201接收这些信号并进行决策融合处理，处理结果为：当前方无障碍物且无弯道时，上位机201输出信号为0000；当前方有障碍物，但大于设定的安全距离时，输出信号为0001；当前方有障碍物，小于设定的安全距离但大于设定的危险距离时，输出信号为0101；当前方有障碍物，且小于设定的危险距离时，输出信号为0111；处理结果通过can总线传输到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的指令，控制机械手302按下启动按钮，另一只
机械臂303推动加速档把，收回机械腿301，加速到轨道车辆规定最高速度，实现轨道车辆直线加速控制；步骤三：弯道处理：当第一双目相机101检测到前方有弯道时，第一双目相机101将感知信息传递给上位机201：当轨道车辆前方无弯道且无障碍物时，上位机201输出信号为0000；当前方有弯道但无障碍物时，上位机201输出信号为1000；当前方有弯道且存在障碍物时，若轨道车辆与障碍物间距离大于安全距离，输出信号为1001，若轨道车辆与障碍物间距离小于安全距离大于危险距离，则输出信号为1101，若距离小于危险距离，则输出信号为1111；决策信号经can总线传递至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的指令，控制一只机械手302按下启动按钮，另一只机械臂303推动加速档把，收回机械腿301，加速到轨道车辆规定转向速度，实现轨道车辆转向控制；当轨道车辆驶过弯道，上位机201输出信号由1000变为0000时，vcu整车控制器202下达指令给下位机203，下位机203解析vcu整车控制器202的指令，控制机械臂303推动加速档把，收回机械腿301，加速到轨道车辆规定最高速度，实现轨道车辆直线加速控制；步骤四：充能处理：当第四双目相机108观测到轨道车辆的能源不足时，向vcu整车控制器202传递信号，vcu整车控制器202处理后发出命令控制机器人驾驶轨道车辆前往充能站。
21.如图4所示，上位机201发出信号传递至vcu整车控制器202，vcu整车控制器202下达指令给下位机203控制伺服电机转动带动机械臂303、机械腿301，同时伺服电机与惯导系统105形成一个闭环控制，从而实现井下轨道车辆的精确控制。
22.对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其它的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。
23.以上所述，仅为本发明的较佳实施例，并不用以限制本发明，凡是依据本发明的技术实质对以上实施例所作的任何细微修改、等同替换和改进，均应包含在本发明技术方案
的保护范围之内。