交通锥倒锥复位控制系统及其复位交通锥的控制方法与流程

本发明涉及交通锥倒锥复位控制技术领域，具体是一种交通锥倒锥复位控制系统及其复位交通锥的控制方法。

背景技术：

高速公路养护管理工作的顺畅进行是保障高速公路可持续发展的必要条件，目前公路养护作业采用了公路工程行业标准《公路养护安全作业规程》(jtgh30-2015)中的作业控制区布置方法，该方法是采用一组由文字提示牌和交通锥将部分路段封闭形成布控区，通过交通锥引导车辆的行驶。

然而由于自然的风力原因或人为方面的行车碰撞、车辆产生的气流影响，交通锥在安放后经常性地出现倒伏、偏离原位的状况，使后续过往车辆驾驶员无法辨别出布控区而驾车进入，产生极大的安全隐患。因此对倒伏、偏离原位置的路锥进行复位是道路维修、养护、交通管制必然要处理的问题。但由于交通锥布控距离长，并且需要长时间对交通锥放置状态进行巡视，采用人工对交通锥进行复位工作的方式，效率较低，首先会带来很大的人力成本问题；其次，由于布控现场交通情况复杂，人员对手动扶正交通锥时因动作未及时或长时间工作倦怠极易导致安全事故的发生，目前已有多起类似事故案例的发生。

当前国内外，尚无针对交通锥扶正而研发设计的机器人，因此一种能够对交通锥进行长时间巡视，判断路锥是否倒伏、偏离原位，并在判断后能够自动进行路锥的扶正和复位工作的可移动机器人是有实际的迫切需要的。

技术实现要素：

本发明的目的是提供一种交通锥倒锥复位控制系统及其复位交通锥的控制方法，以实现交通路锥的自动巡视和自动复位。

为了达到上述目的，本发明所采用的技术方案为：

交通锥倒锥复位控制系统，其特征在于：包括由机器人平台及设置在机器人平台上的机械抓手装置构成的交通锥倒锥复位智能机器人，所述机械抓手装置由机械臂和连接于机械臂执行末端的机械抓手构成，还包括控制器、驱动装置、机器视觉与感知模块装置、人机交互装置，其中：

所述控制器包括微处理器、数据收发模块、图像识别处理模块、机械抓手控制模块、机器人行驶动力控制模块。数据收发模块、图像识别处理模块、机械抓手控制模块、机器人行驶动力控制模块分别通过电信号接入微处理器；

所述驱动装置包括驱动机器人平台行驶的主动力电机、驱动机器人平台转向的转向电机、驱动机械抓手装置中机械臂完成动作的抓手电机。主动力电机、转向电机分别与控制器中的机器人行驶动力控制模块电连接，抓手电机与控制器中的机械抓手控制模块电连接；

所述机器视觉与感知模块装置集成于机器人平台和机械抓手装置，机器视觉与感知模块装置包括全景摄像机、视觉传感器、磁传感器、红外距离传感器、角位移传感器、速度传感器、雷达，其中全景摄像机获取交通锥放置状态，视觉传感器识别倒锥倒置方向，磁传感器测定机器人平台自身与原点磁钉的相对位置，红外距离传感器测定交通锥与机器人相对位置，角位移传感器测定机械抓手装置中机械臂的相对转动角度，速度传感器检测机器人平台移动速度，雷达发现与测定障碍物位置，所述全景摄像机、视觉传感器、磁传感器、红外距离传感器、角位移传感器、速度传感器、雷达分别与控制器中的图像识别处理模块电连接；

所述人机交互装置包括分别内置无线收发模块的显示器和遥控器，人机交互装置通过无线信号与控制器中的数据收发模块数据交互连接。

所述的交通锥倒锥复位控制系统，其特征在于：所述控制器还包括电源管理模块，电源管理模块供电连接至控制器中的微处理器、数据收发模块、图像识别处理模块、机械抓手控制模块、机器人行驶动力控制模块。

所述的交通锥倒锥复位控制系统，其特征在于：所述全景摄像机通过多自由度云台集成于机器人平台。

所述的交通锥倒锥复位控制系统，其特征在于：所述机械抓手装置由多自由度机械臂和连接于多自由度机械臂执行末端的机械抓手构成，其中多自由度机械臂由多个关节臂通过关节转动连接构成，机械抓为双指外夹式机械抓，所述角位移传感器测定每个关节臂的相对转动角度，继而获得机械臂的相对转动角度。

所述的交通锥倒锥复位控制系统，其特征在于：所述控制器中微处理器还电连接有存储模块。

一种交通锥倒锥复位控制系统的复位交通锥的控制方法，其特征在于：包括以下步骤：

s1、确定巡航路线及交通锥拍照；首先，启动红外距离传感器让交通锥倒锥复位智能机器人以固定间距跟随布锥车行进一遍并记忆路线，行进过程中全景摄像机每隔时间t1对交通锥进行一次拍照及存储；

s2、建立巡航坐标系；在原点磁钉处启动交通锥倒锥复位智能机器人，交通锥倒锥复位智能机器人通过自身的磁传感器识别所述原点磁钉，并确定为坐标系的坐标原点；交通锥倒锥复位智能机器人通过红外传感器逐个搜寻交通锥，每搜寻到一个交通锥后，交通锥倒锥复位智能机器人便识别该交通锥，并记录交通锥在巡航坐标系内的位置信息；最终将每个交通锥的相对位置信息均记录在巡航坐标系内；

s3、自动巡航及倒锥识别；每隔时间t2，交通锥倒锥复位智能机器人根据上述巡航坐标系沿固定路线自动巡航；在巡航过程中，控制器的图像识别处理模块获得雷达对路线上的障碍物的实时探测信号，在发现障碍物的情况下控制器的机器人行驶动力控制模块根据障碍物位置信息进行自动绕行避障；在巡航过程中，全景摄像头每隔时间t3对交通锥进行一次拍照并进行倒锥图像识别，判断交通锥姿态是否为倒下，若是倒锥则触发预警命令，将图像传送至人机交互装置的显示器由专人进行辅助判定是否为倒锥，若是倒锥则使用人机交互装置的遥控器向机器人的数据收发模块发送靠近倒锥指令；

s4、定位倒锥；感知系统启动：启动视觉传感器，获取前向信息；启动红外传感器装置，获取近觉信息；启动全景视觉摄像机，获取全景信息；控制器的图像识别处理模块根据所获的上述信息调用精确定位算法对倒锥进行定位，并将定位到的交通锥在巡航坐标系下的准确位置信息传递给控制器的机器人行驶动力控制模块；

s5、机器人移动至倒锥位置；控制器的机器人行驶动力控制模块发送移动指令，交通锥倒锥复位智能机器人根据步骤s4获得的交通锥在巡航坐标系下的准确位置信息，并通过红外距离传感器实时测距，移动至倒锥位置；

s6、机器人初始化；当人机交互装置的遥控器接收到用户发出的复位交通锥的指令后，控制器的机械抓手控制模块控制所有执行机构初始化；

s7、倒锥位置坐标转换；控制器的图像识别处理模块获取由红外距离传感器测得的实时信号，获得倒锥在机器人平台默认坐标系下的相对位置信息，因机械手臂与机器人平台的相对位置是实时测定可知的，进而通过坐标转换，获得倒锥在机械抓手默认坐标系下的相对位置；

s8、倒锥方向识别；控制器的机械抓手控制模块发送机械抓手各个关节臂运动指令，机械抓手各关节臂按照设定动作执行；当机械抓手运动至倒锥位置识别点中心位置正上方固定距离处后停止运动，此时机械抓手及其视觉传感器恰好位于倒锥位置的正上方；控制器的图像识别处理模块获取搭载在机械抓手上方的视觉传感器和红外距离传感器的实时信号，实时校正交通锥至少三个特征位置识别点，基于深度神经元卷曲原理和算法，进而识别倒锥倒置方向；

s9、关节臂旋转；控制器的机械抓手控制模块根据步骤s8所获得的倒锥方向信息，发送机械抓手装置的关节臂旋转指令，定量控制关节臂旋转角度使其与倒锥方向匹配；

s10、抓手张开；控制器的机械抓手控制模块发送机械抓手张开指令；

s11、倒锥复位；控制器的机械抓手控制模块发送机械抓手各个关节臂运动指令，使机械抓手垂直下降规定的固定距离并将倒锥夹取；进而控制器的机械抓手控制模块发送机械抓手各个转轴关节运动指令，使机械抓手垂直上升至规定的固定距离并在垂直方向旋转90度，使倒锥正立；

s12、倒锥放置；交通锥倒锥复位智能机器人返回原先巡航路线，并根据步骤s3所记录的交通锥位置信息，行进过程中，全景摄像头每隔时间t4对交通锥进行拍摄，当识别到初始记录位置没有交通锥时，机器人移动至该位置，并将交通锥下放；下放动作过程同抓取动作过程一致，顺序相反；

s13、机器人继续自动巡航；控制器的机械抓手控制模块发送所有执行机构初始化的指令，交通锥倒锥复位机器人返回至设定巡航路线，继续巡航；

s14、机器人停止工作；在上述任一步骤过程中，若人机交互装置的遥控器接收到停止工作的指令时，控制器的机械抓手控制模块控制所有执行机构初始化，之后控制器的机器人行驶动力控制模块发送返回原点磁钉的指令，机器人沿着巡航路线返回原点磁钉处。

所述的交通锥倒锥复位控制系统的复位交通锥的控制方法，其特征在于：所述步骤s7中，在获得倒锥在机器人平台的默认坐标系下的相对位置坐标后，采用坐标转换的方法，得到倒锥在机械抓手坐标系下的相对位置坐标，坐标转换公式如下：

公式中^ap是倒锥在机械抓手坐标系下的相对位置坐标，^bp是倒锥在机器人平台的默认坐标系下的相对位置坐标，是齐次坐标变换矩阵，齐次坐标变换矩阵计算公式如下：

其中θi表示是机器人各个关节臂中的角位移变量，n表示关节臂的数量。

本发明提出的交通锥倒锥复位控制系统及其复位交通锥的控制方法，通过设置控制器与驱动装置、机器视觉与感知模块装置和人机交互模块装置配合使用，实现了对已放置交通锥的自动巡视检测和自动复位，减少了人工操作也减小了工作人员的安全风险。另外，本发明系统还具有结构简单且安全可靠的优点。

附图说明

图1是本发明系统的结构原理框图。

图2是本发明中机器人巡航发现倒锥处理流程图。

图3是本发明中机械抓手倒锥复位流程图

图4是本发明中机器人停止流程图。

图5是本发明中机器人急停流程图。

具体实施方式

下面结合附图和实施例对本发明进一步说明。

如图1所示，交通锥倒锥复位控制系统，包括由机器人平台及设置在机器人平台上的机械抓手装置构成的交通锥倒锥复位智能机器人，机械抓手装置由机械臂和连接于机械臂执行末端的机械抓手构成，还包括控制器、驱动装置、机器视觉与感知模块装置、人机交互装置，其中：

控制器包括微处理器、数据收发模块、图像识别处理模块、机械抓手控制模块、机器人行驶动力控制模块，数据收发模块、图像识别处理模块、机械抓手控制模块、机器人行驶动力控制模块分别电接入微处理器；

驱动装置包括驱动机器人平台行驶的主动力电机、驱动机器人平台转向的转向电机、驱动机械抓手装置中机械臂完成动作的抓手电机，主动力电机、转向电机分别与控制器中的机器人行驶动力控制模块电连接，抓手电机与控制器中的机械抓手控制模块电连接；

机器视觉与感知模块装置集成于机器人平台和机械抓手装置，机器视觉与感知模块装置包括全景摄像机、视觉传感器、磁传感器、红外距离传感器、角位移传感器、速度传感器、雷达，其中全景摄像机通过多自由度云台集成于机器人平台，全景摄像机获取交通锥放置状态，视觉传感器识别倒锥倒置方向，磁传感器测定机器人平台自身与原点磁钉的相对位置，红外距离传感器测定交通锥与机器人相对位置，角位移传感器测定机械抓手装置中机械臂的相对转动角度，速度传感器检测机器人平台移动速度，雷达发现与测定障碍物位置，所述全景摄像机、视觉传感器、磁传感器、红外距离传感器、角位移传感器、速度传感器、雷达分别与控制器中的图像识别处理模块电连接；

人机交互装置包括分别内置无线收发模块的显示器和遥控器，人机交互装置通过无线信号与控制器中的数据收发模块数据交互连接。

本发明实施例中，显示器内置2.4g无线发射与接收模块，遥控器包括控制按键及2.4g无线发射与接收模块，人机交互装置通过2.4g无线发射与接收模块与控制器的数据收发模块无线连接。

本发明实施例中，采用armcortex-a15内核tegrak1芯片作为微处理器。

本发明实施例中，驱动装置的控制包括控制各执行机构动作的电机、控制机器人平台的主动电机和转向电机；其中，所有电机的控制原理都一样，除了简单的启动、停止和使能等控制，还有为了正确的传送脉冲量数据，统一采用pwm驱动方式，利用微处理器通道机械抓手控制模块分别对控制各执行机构动作的电机、控制机器人平台的主动电机和转向电机进行调频调速控制，驱动装置也会反馈报警与到位信号经过机械抓手控制模块传送给微处理器。

控制器还包括电源管理模块，电源管理模块供电连接至控制器中的微处理器、数据收发模块、图像识别处理模块、机械抓手控制模块、机器人行驶动力控制模块。

本发明实施例中，电源管理模块电连接的电源装置；电源装置可通过上位机控制220v和24v电源通断，达到电源管理的目的。

机械抓手装置由多自由度机械臂和连接于多自由度机械臂执行末端的机械抓手构成，其中多自由度机械臂由多个关节臂通过关节转动连接构成，机械抓为双指外夹式机械抓，角位移传感器测定每个关节臂的相对转动角度，继而获得机械臂的相对转动角度。

如图3所示，本发明实施例中，机械抓手装置由底座、六个关节臂构成的机械臂、机械抓手构成，底座固定在机器人平台上，机械臂中初始段关节臂通过转轴与底座连接，由抓手电机驱动控制，可实现初始段关节臂相对底座的360度旋转；其余5个关节臂依次通过转动关节首尾相连，连接处由相应的抓手电机驱动控制旋转；抓取路锥的机械抓手装配在机械臂中最末端的关节臂的执行末端，连接处由转向电机控制驱动旋转；

本发明实施例中，机械抓手采用双指外夹式机械爪，机械抓手由抓手电机控制张开或收紧，抓手电机通过齿轮传动与机械指动力连接，抓手电机与控制器的机械抓手控制模块电连接；

控制器中微处理器还电连接有存储模块。本发明实施例中为防止掉电后重要数据丢失而影响正常工作，设计了16k的数据存储空间，微处理器通过i2c总线协议访问作为数据存储空间的at24c16芯片，写入机器人平台状态位置、机械臂和机械爪状态位置、路锥状态位置信息。at24c16芯片的1、2、3号脚为i2c地址设置输入脚，将其全部接地，5、6号脚分别接微处理器的数据线sda和时钟线scl，7号脚接地，以禁止芯片写保护。

参照图2和图3，本发明提出的基于上述交通锥倒锥复位控制系统的倒锥复位控制方法的优选实施例，包括以下步骤：

步骤s6，当人机交互装置的遥控器接收到用户发出的复位交通锥的指令后，控制器的机械抓手控制模块控制所有执行机构初始化；

用户可通过人机交互装置的遥控器发出复位交通锥的指令,当人机交互装置接收到用户发出的复位路锥指令，控制器的机械抓手控制模块控制所有执行机构初始化。执行机构包括六个机械臂和其对应的六个转轴关节、抓手和抓手旋转轴。系统初始化主要实现各执行机构的位置校准。

s7，倒锥位置坐标转换；控制器的图像识别处理模块获取由红外距离传感器测得的实时信号，获得倒锥在机器人平台默认坐标系下的相对位置信息,因机械手臂与机器人平台的相对位置是实时测定可知的，进而通过坐标转换，获得倒锥在机械抓手默认坐标系下的相对位置。

s8，倒锥方向识别；控制器的机械抓手控制模块发送机械抓手各个转轴关节运动指令，机械抓手各关节按照设定动作执行。控制器的机械抓手控制模块发送关节转轴1转动指令使机械臂1及其后与之相连的机械臂旋转，待达到第一预设时间时控制器的机械抓手控制模块发送关节转轴2转动指令使机械臂2及其后与之相连的机械臂旋转，同此流程，机械臂3、机械臂4、机械臂5、机械臂6逐次进行预设时间的旋转，使整个机械抓手的位姿到达预设的位置状态后停止运动，此时，机械抓手末端运动至倒锥位置识别点中心位置正上方固定距离处。控制器的图像识别处理模块获取搭载在机械抓手上方的视觉传感器和红外距离传感器的实时信号，实时校正交通锥三个特征位置识别点，基于深度神经元卷曲原理和算法，进而识别倒锥倒置方向。

s9，抓手关节旋转；控制器的机械抓手控制模块根据步骤s12所获得的倒锥方向信息，发送机械臂6末端的抓手转轴关节旋转指令，定量控制关节旋转角度使其与倒锥方向匹配。

s10，抓手张开；控制器的机械抓手控制模块发送机械抓手张开指令。

s11，倒锥复位；控制器的机械抓手控制模块发送机械抓手各个转轴关节运动指令，控制器的机械抓手控制模块发送关节转轴2转动指令使机械臂2及其后与之相连的机械臂旋转，待达到第一预设时间时控制器的机械抓手控制模块发送关节转轴3转动指令使机械臂4及其后与之相连的机械臂旋转，同此流程，机械臂4、机械臂5、机械臂6逐次进行预设时间的旋转，使整个机械抓手的位姿到达预设的位置状态后停止运动，使机械抓手垂直下降规定的固定距离；之后控制器的机械抓手控制模块发送抓手夹紧的指令，将倒锥夹取。进而控制器的机械抓手控制模块发送机械抓手各个转轴关节运动指令，控制器的机械抓手控制模块发送关节转轴2转动指令使机械臂2及其后与之相连的机械臂旋转，待达到第一预设时间时控制器的机械抓手控制模块发送关节转轴3转动指令使机械臂4及其后与之相连的机械臂旋转，同此流程，机械臂4、机械臂5、机械臂6和机械抓手逐次进行预设时间的旋转，使整个机械抓手的位姿到达预设的位置状态后停止运动，使机械抓手垂直上升至规定的固定距离并在垂直方向旋转90度，使倒锥正立。

s12，倒锥放置；交通锥倒锥复位智能机器人返回原先巡航路线，并根据步骤s3所记录的交通锥位置信息，行进过程中，全景摄像头每隔5s对交通锥进行拍摄，当识别到初始记录位置没有交通锥时，机器人移动至该位置，并将交通锥下放。控制器的机械抓手控制模块发送关节转轴1转动指令使机械臂1及其后与之相连的机械臂旋转，待达到第一预设时间时控制器的机械抓手控制模块发送关节转轴2转动指令使机械臂2及其后与之相连的机械臂旋转，同此流程，机械臂3、机械臂4、机械臂5、机械臂6逐次进行预设时间的旋转，使整个机械抓手的位姿到达预设的位置状态后停止运动；机械抓手控制模块发送机械抓手张开指令，放下交通锥。

s13，机械抓手执行机构初始化；控制器的机械抓手控制模块发送所有执行机构初始化的指令。使执行机构包括六个机械臂和其对应的六个转轴关节、抓手和抓手旋转轴位置初始化，以便于机器人平台的移动和下一次复位过程的进行。

参照图3，图3为倒锥复位流程，其可在图2所示的识别倒锥流程之后执行。

进一步地，本交通锥倒锥复位控制系统的倒锥复位控制方法还包括：

步骤s14，当人机交互装置的遥控器接收到停止工作的指令时，控制器的机械抓手控制模块控制所有执行机构初始化，控制器的机器人行驶动力控制模块发送返回原点磁钉的指令，机器人沿着巡航路线返回原点磁钉处，具体流程参照图4；

进一步地，本交通锥倒锥复位控制系统的倒锥复位控制方法还包括：

步骤s15，当控制器未接收人机交互装置发出的控制指令时，控制器发出休眠控制指令至机械驱动装置、机器视觉与感知模块装置和机械抓手装置，以使系统处于低功耗模式；

具体地，步骤s15在复位倒锥流程之前或之后执行均可。

进一步地，本交通锥倒锥复位控制系统的倒锥复位控制方法还包括：

步骤s16，当控制器接收到人机交互装置发出的急停指令时，控制器控制当前作业过程立即停止，当再次收到复位倒锥指令时，控制器控制驱动装置、机器视觉与感知模块装置和机械抓手装置按照停止前状态继续执行，具体流程参照图5；

具体地，步骤s16在复位倒锥流程之后或之前执行均可。

(机械手坐标转换部分)

进一步地，在获得倒锥在机器人平台的默认坐标系下的相对位置坐标后，采用坐标转换的方法，得到倒锥在机械抓手坐标系下的相对位置坐标，坐标转换公式如下：

公式中^ap是倒锥在机械抓手坐标系下的相对位置坐标，^bp是倒锥在机器人平台的默认坐标系下的相对位置坐标，是齐次坐标变换矩阵，齐次坐标变换矩阵计算公式如下：

其中θi表示是机器人各个关节臂中的角位移变量，n表示关节臂的数量。