CN114434448A - 破拆机器人的工作臂控制方法及控制系统 - Google Patents

Abstract

本申请公开了一种破拆机器人的工作臂控制方法及控制系统。工作臂控制方法包括获取工作臂末端在当前状态下在工作臂坐标系下的末端坐标；获取破拆目标点在工作臂坐标系下的破拆坐标；根据末端坐标和破拆坐标获取移动至目标点的运动路径，计算工作臂在运动路径上的多个时间点处的多个工作臂姿态；根据多个时间点处的多个工作臂姿态获取在多个时间点处的液压油缸的活塞杆的目标伸缩量；根据多个时间点处的液压油缸的活塞杆的目标伸缩量获取液压油缸在多个时间点处的液压油的目标流量，基于多个时间点处的液压油的目标流量控制液压油缸动作。通过该方法可以摒弃外装于工作臂上的传感器，避免传感器损坏，降低成本，提高可靠性。

Description

破拆机器人的工作臂控制方法及控制系统

技术领域

本申请涉及破拆作业用工作臂的控制，特别涉及一种破拆机器人的工作臂的控制方法及控制系统。

背景技术

随着机器人技术的发展，特别是工业机械手的广泛应用，工作臂的控制技术日新月异，解放了人力。现有的工作臂控制技术主要面向室内环境，且电机驱动，干扰因素较少，控制精度高。但对于野外工作场合的工作臂控制技术发展缓慢，特别是对于复杂破拆作业环境下多自由度液压工作臂的导引控制技术鲜有报道。并且在多自由度工作臂的姿态测量方面，现有测量仪器主要有：倾角传感器、角度编码器、拉线位移传感器、陀螺仪等精密传感器，且外装于工作臂。但对于破拆作业环境，精密仪器脆弱的本体易损坏且价格高。因此本申请基于液压缸的进出流量与其活塞杆的伸缩量之间的关系，提出了一种新的破拆机器人的工作臂控制方法。

在此需要说明的是，该背景技术部分的陈述仅提供与本申请有关的背景技术，并不必然构成现有技术。

发明内容

本申请提供一种破拆机器人的工作臂控制方法及控制系统，以避免外装于工作臂上的传感器在工作环境中损坏，并提高作业的可靠性。

本申请第一方面提供一种破拆机器人的工作臂控制方法。破拆机器人包括工作臂和用于控制工作臂动作的至少一个液压油缸，工作臂具有用于执行破拆作业的工作臂末端，工作臂控制方法包括获取工作臂末端在当前状态下在工作臂坐标系下的末端坐标；获取破拆目标点在工作臂坐标系下的破拆坐标；根据末端坐标和破拆坐标获取使工作臂末端移动至破拆目标点的运动路径，并基于多自由度机械臂的逆运动学解计算工作臂在运动路径上的多个时间点处的多个工作臂姿态；根据多个时间点处的多个工作臂姿态计算获取在多个时间点处的至少一个液压油缸的活塞杆的目标伸缩量；以及根据多个时间点处的至少一个液压油缸的活塞杆的目标伸缩量获取至少一个液压油缸在多个时间点处的液压油的目标流量，并基于多个时间点处的液压油的目标流量控制至少一个液压油缸动作。

在一些实施例中，根据多个时间点处的至少一个液压油缸的活塞杆的目标伸缩量获取至少一个液压油缸在多个时间点处的液压油的目标流量包括获取至少一个液压油缸的进出流量与活塞杆的伸缩量之间的第一参数关系，并基于第一参数关系计算获得至少一个液压油缸在多个时间点处的液压油的目标流量。

在一些实施例中，破拆机器人还包括流量控制阀组，流量控制阀组用于控制至少一个液压油缸的进出流量。流量控制阀组的阀口开度根据输入电流的大小变化。基于多个时间点处的液压油的目标流量控制至少一个液压油缸动作包括获取流量控制阀组在不同电流大小下的阀口开度，并推导出在不同阀口开度下通过阀口的液压油的流速；以及根据不同电流大小下的液压油的流速和多个时间点处的液压油的目标流量控制流量控制阀组的电流信号及开关时间。

在一些实施例中，获取工作臂末端在当前状态下在工作臂坐标系下的末端坐标包括获取工作臂末端在初始状态下的初始坐标；获取工作臂末端从初始状态到当前状态的过程中通过至少一个液压油缸的实际流量，并根据通过至少一个液压油缸的实际流量获取至少一个液压油缸的活塞杆的实际伸缩量；以及通过初始坐标和至少一个液压油缸的活塞杆的实际伸缩量并基于多自由度机械臂的正运动学解推导出末端坐标。

在一些实施例中，破拆机器人还包括回转马达和与回转马达驱动连接的回转平台。工作臂安装在回转平台上，回转平台用于带动工作臂旋转，获取工作臂末端在当前状态下在工作臂坐标系下的末端坐标还包括获取回转马达的进出流量与回转平台的旋转角度之间的第二参数关系；获取通过回转马达的实际流量，并根据回转马达的实际流量和第二参数关系获取回转平台的实际旋转角度；以及通过初始坐标、至少一个液压油缸的活塞杆的实际伸缩量以及回转平台的实际旋转角度并基于多自由度机械臂的正运动学解推导出末端坐标。

在一些实施例中，获取破拆目标点在工作臂坐标系下的破拆坐标包括通过视觉定位技术获取破拆目标点在工作臂坐标系下的第一坐标；通过激光定位技术获取破拆目标点在工作臂坐标系下的第二坐标；以及将第一坐标和第二坐标加权计算获取破拆目标点在工作臂坐标系下的破拆坐标。

在一些实施例中，将第一坐标和第二坐标加权计算获取破拆目标点在工作臂坐标系下的破拆坐标包括利用加权公式C＝kA1+(1-k)A2计算获取破拆坐标。k为加权参数，根据光照亮度改变加权参数。当光照亮度在[0,10]之间时，加权参数为0。当光照亮度在[10,100]之间时，加权参数为0.25。当光照亮度在[100,500]之间时，加权参数为0.5。当光照亮度在[500,60000]之间时，加权参数为0.75。当光照亮度在[60000,200000]之间时，加权参数为1。

在一些实施例中，根据光照亮度改变加权参数包括使用光照传感器对作业环境进行检测来获取光照亮度，并根据光照传感器获取的光照亮度对加权参数进行调整。

在一些实施例中，通过视觉定位技术获取破拆目标点在工作臂坐标系下的第一坐标包括利用视觉机器获取破拆目标点在视觉坐标系下的视觉坐标，并根据视觉坐标系和工作臂坐标系之间的转换关系将视觉坐标转换为第一坐标。

在一些实施例中，通过激光定位技术获取破拆目标点在工作臂坐标系下的第二坐标包括利用激光测距仪获取破拆目标点在世界坐标系下的世界坐标，并根据世界坐标系和工作臂坐标系之间的转换关系将世界坐标转换为第二坐标。

本申请第二方面提供一种破拆机器人的工作臂控制系统，包括工作臂，具有用于执行破拆作业的工作臂末端；至少一个液压油缸，与工作臂连接以控制工作臂运动；以及控制器，被配置为执行如下步骤，包括获取工作臂的工作臂末端在当前状态下在工作臂坐标系下的末端坐标；获取破拆目标点在工作臂坐标系下的破拆坐标；根据末端坐标和破拆坐标获取使工作臂末端移动至破拆目标点的运动路径，并基于多自由度机械臂的逆运动学解计算工作臂在运动路径上的多个时间点处的多个工作臂姿态；根据多个时间点处的多个工作臂姿态计算获取在多个时间点处至少一个液压油缸的活塞杆的目标伸缩量；以及根据多个时间点处的至少一个液压油缸的活塞杆的目标伸缩量获取至少一个液压油缸在多个时间点处的液压油的目标流量，并基于多个时间点处的液压油的目标流量控制至少一个液压油缸动作。

在一些实施例中，控制器还被配置为获取至少一个液压油缸的进出流量与活塞杆的伸缩量之间的第一参数关系，并根据第一参数关系由至少一个液压油缸的活塞杆的伸缩量获取至少一个液压油缸在多个时间点处的液压油的流量。

在一些实施例中，破拆机器人还包括用于控制至少一个液压油缸的进出流量的流量控制阀组，且流量控制阀组的阀口开度根据输入电流的大小变化。控制器被配置为获取流量控制阀组在不同电流大小下的阀口开度，并推导出在不同阀口开度下通过阀口的液压油的流速。根据不同电流大小下的液压油的流速和多个时间点处的液压油的目标流量控制流量控制阀组的电流信号及开关时间。

在一些实施例中，流量控制阀组包括电磁比例阀和多路阀。多路阀包括与至少一个液压油缸对应连接的至少一个液压通道。至少一个液压通道的阀口开度根据电磁比例阀的电流信号变化。控制器被配置为控制电磁比例阀的电流信号以控制至少一个液压通道的阀口开度。

在一些实施例中，控制器被配置为在破拆机器人启动时，获取工作臂末端在初始状态下的初始坐标，且控制器还被配置为获取工作臂末端从初始状态到当前状态的过程中通过至少一个液压油缸的实际流量，并根据通过至少一个液压油缸的实际流量获取至少一个液压油缸的活塞杆的实际伸缩量，然后通过初始坐标和至少一个液压油缸的活塞杆的实际伸缩量并基于多自由度机械臂的正运动学解推导出末端坐标。

在一些实施例中，破拆机器人还包括回转马达和由回转马达驱动的回转平台。工作臂安装在回转平台上，回转平台用于带动工作臂旋转。控制器被配置为获取回转马达的进出流量与回转平台的旋转角度之间的第二参数关系，获取通过回转马达的实际流量，通过初始坐标、至少一个液压油缸的活塞杆的实际伸缩量以及回转平台的实际旋转角度并基于多自由度机械臂的正运动学解推导出末端坐标。

在一些实施例中，破拆机器人还包括与控制器电连接的复合定位装置。复合定位装置用于获取破拆目标点在视觉坐标系下的视觉坐标和在世界坐标系下的世界坐标并将坐标数据发送给控制器，控制器被配置为将视觉坐标和世界坐标转换为破拆坐标。

在一些实施例中，复合定位装置包括二维云台和设置在二维云台上的视觉机器及激光测距仪。破拆机器人还包括光照传感器。其中，二维云台可动地设置以使破拆目标点成像于视觉机器的视场范围内且使激光测距仪的光斑位于破拆目标点上，且二维云台被配置为向控制器发送俯仰角度数据和旋转角度数据；视觉机器被配置为获取破拆目标点在视觉坐标系下的视觉坐标且将视觉坐标发送至控制器；激光测距仪被配置为获取破拆目标点与激光测距仪之间的距离并向控制器发送距离数据；光照传感器被配置为获取作业环境的光照亮度并将光照亮度发送至控制器；控制器被配置为根据距离数据、俯仰角度数据和旋转角度数据计算得到破拆目标点在世界坐标系下的世界坐标，控制器还被配置为将视觉坐标和世界坐标分别转换为工作臂坐标系下的第一坐标和第二坐标，并利用加权公式C＝kA1+(1-k)A2计算获取破拆坐标，k为加权参数，加权参数根据光照亮度改变，当光照亮度在[0,10]之间时，加权参数为0，当光照亮度在[10,100]之间时，加权参数为0.25，当光照亮度在[100,500]之间时，加权参数为0.5，当光照亮度在[500,60000]之间时，加权参数为0.75，当光照亮度在[60000,200000]之间时，加权参数为1。

在一些实施例中，视觉机器包括双目相机、单目相机、景深相机、结构光相机中的至少一种。

基于本申请提供的技术方案，破拆机器人包括工作臂和用于控制工作臂动作的至少一个液压油缸，工作臂的控制方法包括获取工作臂末端在当前状态下在工作臂坐标系下的末端坐标；获取破拆目标点在工作臂坐标系下的破拆坐标；根据末端坐标和破拆坐标获取使工作臂末端移动至破拆目标点的运动路径，并基于多自由度机械臂的逆运动学解计算工作臂在运动路径上的多个时间点处的多个工作臂姿态；根据多个时间点处的多个工作臂姿态计算获取在多个时间点处的至少一个液压油缸的活塞杆的伸缩量；以及根据多个时间点处的至少一个液压油缸的活塞杆的伸缩量获取至少一个液压油缸在多个时间点处的液压油的流量，并基于多个时间点处的液压油的流量控制至少一个液压油缸动作。通过该方法可以摒弃传统的外装于工作臂上的传感器，避免了传感器在工作环境中遭受损坏，降低了整机成本，同时提高工作臂控制的可靠性。

通过以下参照附图对本申请的示例性实施例的详细描述，本申请的其它特征及其优点将会变得清楚。

附图说明

此处所说明的附图用来提供对本申请的进一步理解，构成本申请的一部分，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：

图1为本申请实施例的破拆机器人的结构示意图。

图2为本申请实施例的破拆机器人的工作臂控制方法的流程图。

图3为本申请又一实施例的破拆机器人的工作臂控制系统的工作原理示意图。

图中：1、视觉机器；2、激光测距仪；3、二维云台；4、光照传感器；5、控制器；6、电磁比例阀；7、多路阀；8、回转马达；9、工作臂；10、回转平台；11、液压油缸；12、工作臂末端。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本申请及其应用或使用的任何限制。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本申请的范围。同时，应当明白，为了便于描述，附图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，技术、方法和设备应当被视为说明书的一部分。在这里示出和讨论的所有示例中，任何具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它示例可以具有不同的值。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。

为了便于描述，在这里可以使用空间相对术语，如“在……之上”、“在……上方”、“在……上表面”、“上面的”等，用来描述如在图中所示的一个器件或特征与其他器件或特征的空间位置关系。应当理解的是，空间相对术语旨在包含除了器件在图中所描述的方位之外的在使用或操作中的不同方位。例如，如果附图中的器件被倒置，则描述为“在其他器件或构造上方”或“在其他器件或构造之上”的器件之后将被定位为“在其他器件或构造下方”或“在其他器件或构造之下”。因而，示例性术语“在……上方”可以包括“在……上方”和“在……下方”两种方位。该器件也可以其他不同方式定位，并且对这里所使用的空间相对描述作出相应解释。

参考图1，破拆机器人包括回转平台10、工作臂9以及用于控制工作臂9动作的至少一个液压油缸11。其中在图1示出的具体实施例中，回转平台10可转动地设置。工作臂9包括第一节臂、第二节臂、第三节臂和破拆工具，其中第一节臂可转动地连接在回转平台10上，第二节臂的第一端可转动地与第一节臂连接，第二节臂的第二端可转动地与第三节臂连接，第三节臂上设置有破拆工具，如图1中所示，破拆工具为钻头，本文所述的工作臂末端12指的是钻头的端部。至少一个液压油缸11包括用于驱动第一节臂转动的第一液压油缸、用于驱动第二节臂转动的第二液压油缸和第三液压油缸以及用于驱动第三节臂以及搭载在第三节臂上的破拆工具(在本实施例中，为钻头)转动的第四液压油缸。工作臂末端的位置(钻头端部的位置)由以上各个液压油缸的活塞杆的伸缩量以及回转平台10的回转角度共同决定，因此为了对工作臂末端12的位置进行精确控制，需要对各个液压油缸的活塞杆的伸缩量和回转平台的回转角度进行精确控制。

为了提高对工作臂末端12的位置控制的精确度，本申请提供一种破拆机器人的工作臂的控制方法，参考图2，本申请实施例的工作臂的控制方法包括如下步骤：

S510，获取工作臂末端12在当前状态下在工作臂坐标系下的末端坐标B；

S520，获取破拆目标点在工作臂坐标系下的破拆坐标C；

S530，根据末端坐标B和破拆坐标C获取使工作臂末端12移动至破拆目标点的运动路径，并基于多自由度机械臂的逆运动学解计算工作臂9在运动路径上的多个时间点处的多个工作臂姿态；

S540，根据多个时间点处的多个工作臂姿态计算获取在多个时间点处的至少一个液压油缸11的活塞杆的目标伸缩量；以及

S550，根据多个时间点处的至少一个液压油缸11的活塞杆的目标伸缩量获取至少一个液压油缸11在多个时间点处的液压油的目标流量，并基于多个时间点处的液压油的目标流量控制至少一个液压油缸11动作。

本申请实施例的控制方法，通过工作臂姿态获取至少一个液压油缸11的活塞杆的目标伸缩量，并根据目标伸缩量推导出通过至少一个液压油缸11的液压油的目标流量，然后根据目标流量来对液压油缸的实际流量进行控制即可实现对工作臂9的动作的精确控制。与相关技术中通过外装于工作臂上的传感器来对液压油缸的伸缩量进行检测和控制相比，有效避免传感器在工作环境中遭受损坏，降低了整机成本，同时提高了控制的可靠性。

在图1示出的具体实施例中，该破拆机器人的工作臂为多节臂，相应的，该破拆机器人包括用于驱动多节臂动作的多个液压油缸11。那么在获取工作臂姿态后就需要根据工作臂姿态获取多个液压油缸11的活塞杆的目标伸缩量，并针对每一个液压油缸11计算其目标流量以对每一个液压油缸11进行分别控制。当然在其他实施例中，破拆机器人的工作臂也可以为单节臂，此时该单节臂可以由单个液压油缸来驱动，也可以由两个以上的液压油缸来驱动。

在一些实施例中，根据多个时间点处的至少一个液压油缸11的活塞杆的目标伸缩量获取至少一个液压油缸11在多个时间点处的液压油的目标流量包括获取至少一个液压油缸11的进出流量与活塞杆的伸缩量之间的第一参数关系，并基于该第一参数关系进行计算。具体地，在控制器5中标定至少一个液压油缸11的进出流量与其活塞杆的伸缩量的对应关系，通过测定液压油的流量即可获取活塞杆的对应伸缩量。此处获取液压油缸的进出流量与其活塞杆的伸缩量之间的第一参数关系可以通过预先进行试验来测定。

在一些实施例中，破拆机器人还包括流量控制阀组，流量控制阀组用于控制至少一个液压油缸11的进出流量，流量控制阀组的阀口开度根据输入电流的大小变化，基于多个时间点处的液压油的目标流量控制至少一个液压油缸11动作包括获取流量控制阀组在不同电流大小下的阀口开度，并推导出在不同阀口开度下通过阀口的液压油的流速；以及根据不同电流大小下的液压油的流速和多个时间点处的液压油的目标流量控制流量控制阀组的电流信号及开关时间，以使通过至少一个液压油缸11的实际流量满足活塞杆目标伸缩量的需求。

具体地，在控制器5中标定流量控制阀组的阀口开度与输入电流的大小之间的关系。更具体地，在本实施例中，流量控制阀组的阀口开度与输入电流的大小成比例。然后在阀口两端压力稳定的情况下，即可获得在不同阀口开度下的液压油的流速。控制器5通过控制流量控制阀组的输入电流的大小以及开关时间，即可控制液压油的流量。换句话说，通过该方法可以实现对通过至少一个液压油缸11的液压油的流量进行控制，以使工作臂达到目标姿态，从而更加简便地实现对工作臂动作的控制。

在一些实施例中，获取工作臂末端12在当前状态下在工作臂坐标系下的末端坐标B包括获取工作臂末端12在初始状态下的初始坐标b；获取工作臂末端12从初始状态到当前状态的过程中通过至少一个液压油缸11的实际流量，并根据通过至少一个液压油缸11的实际流量获取至少一个液压油缸11的活塞杆的实际伸缩量；通过初始坐标b和至少一个液压油缸11的活塞杆的实际伸缩量并基于多自由度机械臂的正运动学解推导出末端坐标B。

具体地，工作臂末端12的初始坐标b指的是破拆机器人启动时工作臂末端12在工作臂坐标系下的坐标，因此初始坐标b是固定值，每次破拆机器人启动工作时，其初始坐标b是确定的，被记录在控制器5中。那么当破拆机器人在刚启动而进行破拆作业时，初始坐标b即为工作臂末端12在当前状态下在工作臂坐标系下的末端坐标。此时可以直接根据初始坐标b和破拆坐标C进行运动路径的规划。当破拆机器人完成某一目标点的破拆作业后，要继续进行下一个目标点的破拆作业时，那么此时就要通过初始坐标b计算工作臂末端12在当前状态下在工作臂坐标系的末端坐标B。具体地，基于上一工作阶段记录的通过至少一个液压油缸11的实际流量，获取在当前状态下至少一个液压油缸11的活塞杆的实际伸缩量，然后在初始坐标b的基础上，基于多自由度机械臂的正运动学解推导出工作臂末端12在当前状态下的末端坐标B。因此该破拆机器人在完成某一破拆点的作业后，其工作臂末端12的末端坐标可实时获取到，进而可根据该末端坐标B和下一个破拆目标点在工作臂坐标系下的破拆坐标计算运动路径，控制工作臂9从当前状态继续进行下一阶段的破拆作业，提升破拆效率。

在一些实施例中，破拆机器人还包括回转马达8和与回转马达8驱动连接的回转平台10。工作臂9安装在回转平台10上，回转平台10用于带动工作臂9旋转。获取末端坐标B还包括获取回转马达8的进出流量与回转平台10的旋转角度之间的第二参数关系；获取通过回转马达8的实际流量，并根据回转马达8的实际流量和第二参数关系获取回转平台10的实际旋转角度；以及通过初始坐标b、至少一个液压油缸11的活塞杆的实际伸缩量以及回转平台10的实际旋转角度并基于多自由度机械臂的正运动学解推导出末端坐标B。

具体地，回转平台10用于带动工作臂9在水平面内绕竖直轴旋转，进而增加工作臂9在空间中运动的自由度。进一步地，流量控制阀组与回转马达8连接，在控制器5中标定回转马达8的进出流量与回转平台10的旋转角度之间的第二参数关系，通过测定液压油的流量即可获取回转平台10的对应旋转角度，然后便能获得工作臂末端12在三维空间中的末端坐标B。此处获取回转马达的进出流量与回转平台的旋转角度之间的第二参数关系可以通过预先进行试验来测定。

在一些实施例中，获取破拆目标点在工作臂坐标系下的破拆坐标C包括通过视觉定位技术获取破拆目标点在工作臂坐标系下的第一坐标A1；通过激光定位技术获取破拆目标点在工作臂坐标系下的第二坐标A2；然后将第一坐标A1和第二坐标A2加权计算获取破拆目标点在工作臂坐标系下的破拆坐标C。将两种定位技术结合起来进行破拆目标点的定位，可以有效地提高破拆目标点定位的可靠性。

进一步地，将第一坐标A1和第二坐标A2加权计算获取破拆目标点在工作臂坐标系下的破拆坐标C包括利用加权公式C＝kA1+(1-k)A2计算获取破拆坐标C，k为加权参数，根据光照亮度改变加权参数，当光照亮度在[0,10]之间时，加权参数为0，当光照亮度在[10,100]之间时，加权参数为0.25，当光照亮度在[100,500]之间时，加权参数为0.5，当光照亮度在[500,60000]之间时，加权参数为0.75，当光照亮度在[60000,200000]之间时，加权参数为1。也就是说本申请实施例的控制方法根据天气光照变化，赋予其不同的权重，提高破拆目标点定位的可靠性。

在一些实施例中，当光照亮度在[0,10]之间时，将作业环境定义为“黑”，当光照亮度在[10,100]之间时，将作业环境定义为“暗”，当光照亮度在[100,500]之间时，将作业环境定义为“正常”，当光照亮度在[500,60000]之间时，将作业环境定义为“亮”，当光照亮度在[60000,200000]之间时，将作业环境定义为“极亮”。

具体地，可以在光照亮度不足的情况下，提高激光定位的权重，降低视觉定位的权重。而在光照亮度充足的情况下，提高视觉定位的权重，降低激光定位的权重。例如，当光照亮度在[0,10]之间时，此时可以将作业环境定义为“黑”，并且此时的加权参数为0。即，在“黑”的作业环境下，破拆坐标C完全取决于激光定位技术。而当光照亮度在[100,500]之间时，此时可以将作业环境定义为“正常”，并且此时的加权参数为0.5。即，破拆坐标C由视觉定位技术和激光定位技术共同决定，且两者在定位过程中的权重相同。因此，使用该复合定位方法，既可以解决在对隧道进行破拆作业时的粉尘较大，光照不足等问题，也可以解决在室外进行破拆作业时，天气变化对激光测量设备的精准度影响的问题。

在一些实施例中，根据光照亮度改变加权参数包括使用光照传感器4对作业环境进行检测来获取光照亮度，并根据光照传感器4获取的光照亮度对加权参数进行调整。

在一些实施例中，通过视觉定位技术获取破拆目标点在工作臂坐标系下的第一坐标A1包括利用视觉机器1获取破拆目标点在视觉坐标系下的视觉坐标a1，并根据视觉坐标系和工作臂坐标系之间的转换关系将视觉坐标a1转换为第一坐标A1。

在一些实施例中，通过激光定位技术获取破拆目标点在工作臂坐标系下的第二坐标A2包括利用激光测距仪2获取破拆目标点在世界坐标系下的世界坐标a2，并根据世界坐标系和工作臂坐标系之间的转换关系将世界坐标a2转换为第二坐标A2。

在一些实施例中，在控制器5中建立视觉坐标系、世界坐标系及工作臂坐标系之间的转换关系，控制器5将不同坐标系下的坐标进行转换。

本申请还提供了一种破拆机器人的工作臂控制系统，参考图1和图3，包括工作臂9、至少一个液压油缸11以及控制器5。其中，工作臂9具有用于执行破拆作业的工作臂末端12。至少一个液压油缸11与工作臂9连接以控制工作臂9运动。控制器5被配置为执行如下步骤：获取工作臂9的工作臂末端12在当前状态下在工作臂坐标系下的末端坐标B；获取破拆目标点在工作臂坐标系下的破拆坐标C；根据末端坐标B和破拆坐标C获取使工作臂末端12移动至破拆目标点的运动路径，并基于多自由度机械臂的逆运动学解计算工作臂9在运动路径上的多个时间点处的多个工作臂姿态；根据多个时间点处的多个工作臂姿态计算获取在多个时间点处至少一个液压油缸11的活塞杆的目标伸缩量；以及根据多个时间点处的至少一个液压油缸11的活塞杆的目标伸缩量获取至少一个液压油缸11在多个时间点处的液压油的目标流量，并基于多个时间点处的液压油的目标流量控制至少一个液压油缸11动作。

与相关技术中通过外装于工作臂上的传感器来对来对工作臂姿态进行检测和控制相比，有效避免传感器在工作环境中遭受损坏，降低了整机成本，同时提高了控制的可靠性。即通过本申请实施例的控制系统，仅需获取液压油缸11的活塞杆的目标伸缩量，并根据该目标伸缩量推导出通过液压油缸的液压油的目标流量，随后控制实际流量以满足需求即可实现对工作臂9的动作的精确控制。

在一些实施例中，控制器5还被配置为获取至少一个液压油缸11的进出流量与活塞杆的伸缩量之间的第一参数关系，并根据该第一参数关系由至少一个液压油缸11的活塞杆的伸缩量获取至少一个液压油缸11在多个时间点处的液压油的流量。

在一些实施例中，破拆机器人还包括流量控制阀组，流量控制阀组用于控制至少一个液压油缸11的进出流量，且流量控制阀组的阀口开度根据输入电流的大小变化，控制器5被配置为获取流量控制阀组在不同电流大小下的阀口开度，并推导出在不同阀口开度下通过阀口的液压油的流速，根据不同电流大小下的液压油的流速和多个时间点处的液压油的目标流量控制流量控制阀组的电流信号及开关时间，以使通过至少一个液压油缸11的实际流量满足活塞杆的目标伸缩量的需求。

具体地，在控制器5中标定流量控制阀组的阀口开度与输入电流的大小之间的第一参数关系(此处同上，可以通过预先进行试验来测定)。更具体地，在本实施例中，流量控制阀组的阀口开度与输入电流的大小成比例。然后在阀口两端压力稳定的情况下，即可获得在不同阀口开度下的液压油的流速。控制器5通过控制流量控制阀组的输入电流的大小以及开关时间，即可控制液压油的实际流量。换句话说，通过该方法可以实现对通过液压油缸11的液压油的实际流量进行控制，以使液压油缸11的活塞杆的实际伸缩量满足需求，进而使工作臂9达到目标姿态，从而更加简便地实现对工作臂动作的控制。

在一些实施例中，流量控制阀组包括电磁比例阀6和多路阀7，多路阀7包括与至少一个液压油缸11对应连接的至少一个液压通道，至少一个液压通道的阀口开度根据电磁比例阀6的电流信号变化，控制器5被配置为控制电磁比例阀6的电流信号以控制至少一个液压通道的阀口开度。

在一些实施例中，控制器5被配置为在破拆机器人启动时，获取工作臂末端12在初始状态下的初始坐标b，且控制器5还被配置为获取工作臂末端12从初始状态到当前状态的过程中通过至少一个液压油缸11的实际流量，并根据至少一个液压油缸11的实际流量获取至少一个液压油缸11的活塞杆的实际伸缩量，并通过初始坐标b和至少一个液压油缸11的活塞杆的实际伸缩量并基于多自由度机械臂的正运动学解推导出末端坐标B。

具体地，工作臂末端12的初始坐标b指的是破拆机器人启动时工作臂末端12在工作臂坐标系下的坐标，因此初始坐标b是固定值，每次破拆机器人启动工作时，其初始坐标b是确定的，被记录在控制器5中。那么当破拆机器人在刚启动而进行破拆作业时，初始坐标b即为工作臂末端12在当前状态下在工作臂坐标系下的末端坐标。此时可以直接根据初始坐标b和破拆坐标C进行运动路径的规划。当破拆机器人完成某一目标点的破拆作业后，要继续进行下一个目标点的破拆作业时，那么此时就要通过初始坐标b计算工作臂末端12在当前状态下在工作臂坐标系的末端坐标B。具体地，基于上一工作阶段记录的通过至少一个液压油缸11的实际流量，获取在当前状态下至少一个液压油缸11的活塞杆的实际伸缩量，然后在初始坐标b的基础上，基于多自由度机械臂的正运动学解推导出工作臂末端12在当前状态下的末端坐标B。因此该破拆机器人在完成某一破拆点的作业后，其工作臂末端的末端坐标可实时获取到，进而可根据该末端坐标和下一个破拆目标点在工作臂坐标系下的破拆坐标计算运动路径，控制工作臂从当前状态继续进行下一阶段的破拆作业，提升破拆效率

在一些实施例中，破拆机器人还包括回转马达8和由回转马达8驱动的回转平台10，工作臂9安装在回转平台10上，回转平台10用于带动工作臂9旋转，控制器5被配置为获取回转马达8的进出流量与回转平台10的旋转角度之间的第二参数关系，获取通过回转马达8的实际流量，通过初始坐标b、至少一个液压油缸11的活塞杆的实际伸缩量以及回转平台10的实际旋转角度并基于多自由度机械臂的正运动学解推导出末端坐标B。

具体地，回转平台10用于带动工作臂9在水平面内绕竖直轴旋转，进而增加工作臂9在空间中运动的自由度。进一步地，流量控制阀组与回转马达8连接，在控制器5中标定回转马达8的进出流量与回转平台10的旋转角度之间的第二参数关系(此处同上，可以通过预先进行试验来测定)，通过测定液压油的流量即可获取回转平台10的对应旋转角度，然后便能获得工作臂末端12在三维空间中的末端坐标B。

在一些实施例中，破拆机器人还包括与控制器5电连接的复合定位装置，复合定位装置用于获取破拆目标点在视觉坐标系下的视觉坐标a1和在世界坐标系下的世界坐标a2并将坐标数据发送给控制器5，控制器5被配置为将视觉坐标a1和世界坐标a2转换为破拆坐标C。

进一步地，复合定位装置包括二维云台3和设置在二维云台3上的视觉机器1及激光测距仪2，破拆机器人还包括光照传感器4。其中，二维云台3可动地设置以使破拆目标点成像于视觉机器1的视场范围内且使激光测距仪2的光斑位于破拆目标点上。二维云台3被配置为向控制器5发送俯仰角度数据和旋转角度数据。视觉机器1被配置为获取破拆目标点在视觉坐标系下的视觉坐标a1且将视觉坐标a1发送至控制器5。激光测距仪2被配置为获取破拆目标点与激光测距仪2之间的距离并向控制器5发送距离数据。光照传感器4被配置为获取作业环境的光照亮度并将光照亮度发送至控制器5。控制器5被配置为根据距离数据、俯仰角度数据和旋转角度数据计算得到破拆目标点在世界坐标系下的世界坐标a2，控制器5还被配置为将视觉坐标a1和世界坐标a2分别转换为工作臂坐标系下的第一坐标A1和第二坐标A2，并利用加权公式C＝kA1+(1-k)A2计算获取破拆坐标C，k为加权参数，加权参数根据光照亮度改变，当光照亮度在[0,10]之间时，加权参数为0，当光照亮度在[10,100]之间时，加权参数为0.25，当光照亮度在[100,500]之间时，加权参数为0.5，当光照亮度在[500,60000]之间时，加权参数为0.75，当光照亮度在[60000,200000]之间时，加权参数为1。

具体地，可以在光照亮度不足的情况下，提高激光定位的权重，降低视觉定位的权重。而在光照亮度充足的情况下，提高视觉定位的权重，降低激光定位的权重。例如，当光照亮度在[0,10]之间时，此时可以将作业环境定义为“黑”，并且此时的加权参数为0。即，在“黑”的作业环境下，破拆坐标C完全取决于激光定位技术。而当光照亮度在[100,500]之间时，此时可以将作业环境定义为“正常”，并且此时的加权参数为0.5。即，破拆坐标C由视觉定位技术和激光定位技术共同决定，且两者在定位过程中的权重相同。因此，使用该复合定位方法，既可以解决在对隧道进行破拆作业时的粉尘较大，光照不足等问题，也可以解决在室外进行破拆作业时，天气变化对激光测量设备的精准度影响的问题。

当然，该复合定位装置也可以包括三维云台，此处不做限定。

在一些实施例中，破拆机器人还包括CAN通信模块。CAN通信模块与控制器5、视觉机器1、二维云台3、激光测距仪2、光照传感器4均信号连接，以在各个部分之间快速地传递数据。

在一些实施例中，视觉机器1包括双目相机、单目相机、景深相机、结构光相机中的至少一种。

具体在本实施例中，视觉机器1为双目相机，且双目相机和激光测距仪2均位于高精度二维云台3上，且激光测距仪2位于双目相机的中心。

下面将详细描述本申请实施例的破拆机器人的作业过程。参考图1，本申请的破拆机器人的破拆用工作臂是油缸驱动的三段式四自由度机械臂。破拆机器人在开机时，首先记录工作臂末端12在工作臂坐标系下的初始坐标b，然后操作人员操作高精度二维云台3，使破拆目标点清晰地成像于双目相机1的机器视场内，且激光测距仪2的光斑准确地位于破拆目标点上。然后控制器5通过双目相机1获取破拆目标点在视觉坐标系下的视觉坐标a1。同时，控制器5读取高精度二维云台3的俯仰角度数据和旋转角度数据并结合激光测距仪2的距离数据，获取破拆目标点在世界坐标系下的世界坐标a2。然后控制器5将视觉坐标a1和世界坐标a2全部转换到工作臂坐标系下的第一坐标A1和第二坐标A2。此时，光照传感器4对作业环境的光照亮度进行检测，并将光照数据传输到控制器5中，控制器5根据光照数据调整加权参数，并且依据加权公式C＝kA1+(1-k)A2计算获取破拆坐标C。例如，当光照亮度在[0,10]之间时，将此作业环境定义为“黑”，控制器5将加权参数调整为0，此时破拆坐标C就是根据世界坐标a2获取的第二坐标A2。换句话说，此时破拆坐标C完全由激光定位技术来决定。而当光照亮度在[100,500]之间时，将此作业环境定义为“正常”，控制器5将加权参数调整为0.5。即，破拆坐标C由视觉定位技术和激光定位技术共同决定，且两者在定位过程中的权重相同。接着，控制器5根据工作臂9的初始坐标b和破拆坐标C获取使工作臂末端12移动至破拆目标点的运动路径，并基于多自由度机械臂的逆运动学解计算工作臂9在该运动路径上的多个时间点处的多个工作臂姿态。根据计算出的多个时间点处的工作臂姿态推算出回转平台10的目标旋转角度以及四个液压油缸11的活塞杆的目标伸缩量，控制器5依据液压油缸11的进出流量与其活塞杆伸缩量之间的第一参数关系以及回转马达8的进出流量与回转平台10的旋转角度之间的第二参数关系，推算出四个液压油缸11目标流量和回转马达8的目标流量，然后控制电磁比例阀6的电流信号以及开关时间，以使通过四个液压油缸11的实际流量和通过回转马达8的实际流量满足目标流量需求，从而使工作臂末端12移动至目标点进行破拆作业。在完成第一阶段的破拆作业后，如果需要更换破拆目标，则控制器5基于初始坐标b和当前状态下回转马达8的进出流量以及四个液压油缸11的进出流量，推算出工作臂末端12在当前状态下的末端坐标B，然后重复上述流程以进行第二阶段破拆作业。

最后应当说明的是：以上实施例仅用以说明本申请的技术方案而非对其限制；尽管参照较佳实施例对本申请进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本申请的具体实施方式进行修改或者对部分技术特征进行等同替换；而不脱离本申请技术方案的精神，其均应涵盖在本申请请求保护的技术方案范围当中。

Claims (19)

1.一种破拆机器人的工作臂控制方法，所述破拆机器人包括工作臂(9)和用于控制所述工作臂(9)动作的至少一个液压油缸(11)，所述工作臂(9)具有用于执行破拆作业的工作臂末端(12)，其特征在于，所述工作臂控制方法包括如下步骤：

获取所述工作臂末端(12)在当前状态下在工作臂坐标系下的末端坐标(B)；

获取破拆目标点在工作臂坐标系下的破拆坐标(C)；

根据所述末端坐标(B)和所述破拆坐标(C)获取使所述工作臂末端(12)移动至所述破拆目标点的运动路径，并基于多自由度机械臂的逆运动学解计算所述工作臂(9)在所述运动路径上的多个时间点处的多个工作臂姿态；

根据所述多个时间点处的多个工作臂姿态计算获取在多个时间点处的所述至少一个液压油缸(11)的活塞杆的目标伸缩量；以及

根据所述多个时间点处的所述至少一个液压油缸(11)的活塞杆的目标伸缩量获取所述至少一个液压油缸(11)在多个时间点处的液压油的目标流量，并基于所述多个时间点处的液压油的目标流量控制所述至少一个液压油缸(11)动作。

2.根据权利要求1所述的破拆机器人的工作臂控制方法，其特征在于，所述根据多个时间点处的至少一个液压油缸(11)的活塞杆的目标伸缩量获取至少一个液压油缸(11)在多个时间点处的液压油的目标流量包括：获取所述至少一个液压油缸(11)的进出流量与活塞杆的伸缩量之间的第一参数关系，并基于所述第一参数关系计算获得所述至少一个液压油缸(11)在多个时间点处的液压油的目标流量。

3.根据权利要求1所述的破拆机器人的工作臂控制方法，其特征在于，所述破拆机器人还包括流量控制阀组，所述流量控制阀组用于控制所述至少一个液压油缸(11)的进出流量，所述流量控制阀组的阀口开度根据输入电流的大小变化，所述基于多个时间点处的液压油的目标流量控制所述至少一个液压油缸(11)动作包括：

获取所述流量控制阀组在不同电流大小下的阀口开度，并推导出在不同阀口开度下通过所述阀口的液压油的流速；以及

根据所述不同电流大小下的液压油的流速和所述多个时间点处的液压油的目标流量控制所述流量控制阀组的电流信号及开关时间。

4.根据权利要求1所述的破拆机器人的工作臂控制方法，其特征在于，所述获取工作臂末端(12)在当前状态下在工作臂坐标系下的末端坐标(B)包括：

获取所述工作臂末端(12)在初始状态下的初始坐标(b)；

获取所述工作臂末端(12)从所述初始状态到所述当前状态的过程中通过所述至少一个液压油缸(11)的实际流量，并根据通过至少一个液压油缸(11)的实际流量获取所述至少一个液压油缸(11)的活塞杆的实际伸缩量；以及

通过所述初始坐标(b)和所述至少一个液压油缸(11)的活塞杆的实际伸缩量并基于多自由度机械臂的正运动学解推导出所述末端坐标(B)。

5.根据权利要求4所述的破拆机器人的工作臂控制方法，其特征在于，所述破拆机器人还包括回转马达(8)和与所述回转马达(8)驱动连接的回转平台(10)，所述工作臂(9)安装在所述回转平台(10)上，所述回转平台(10)用于带动所述工作臂(9)旋转，所述获取工作臂末端(12)在当前状态下在工作臂坐标系下的末端坐标(B)还包括：

获取所述回转马达(8)的进出流量与所述回转平台(10)的旋转角度之间的第二参数关系；

获取通过所述回转马达(8)的实际流量，并根据所述回转马达(8)的实际流量和所述第二参数关系获取所述回转平台(10)的实际旋转角度；以及

通过所述初始坐标(b)、所述至少一个液压油缸(11)的活塞杆的实际伸缩量以及所述回转平台(10)的实际旋转角度并基于多自由度机械臂的正运动学解推导出所述末端坐标(B)。

6.根据权利要求1-5中任一项所述的破拆机器人的工作臂控制方法，其特征在于，所述获取破拆目标点在工作臂坐标系下的破拆坐标(C)包括：通过视觉定位技术获取所述破拆目标点在工作臂坐标系下的第一坐标(A1)；通过激光定位技术获取所述破拆目标点在所述工作臂坐标系下的第二坐标(A2)；以及将所述第一坐标(A1)和所述第二坐标(A2)加权计算获取所述破拆目标点在所述工作臂坐标系下的破拆坐标(C)。

7.根据权利要求6所述的破拆机器人的工作臂控制方法，其特征在于，将所述第一坐标(A1)和所述第二坐标(A2)加权计算获取所述破拆目标点在所述工作臂坐标系下的破拆坐标(C)包括：利用加权公式C＝kA1+(1-k)A2计算获取所述破拆坐标(C)，k为加权参数，根据光照亮度改变所述加权参数，当所述光照亮度在[0,10]之间时，所述加权参数为0，当所述光照亮度在[10,100]之间时，所述加权参数为0.25，当所述光照亮度在[100,500]之间时，所述加权参数为0.5，当所述光照亮度在[500,60000]之间时，所述加权参数为0.75，当所述光照亮度在[60000,200000]之间时，所述加权参数为1。

8.根据权利要求7所述的破拆机器人的工作臂控制方法，其特征在于，所述根据光照亮度改变所述加权参数包括：使用光照传感器(4)对作业环境进行检测来获取光照亮度，并根据所述光照传感器(4)获取的光照亮度对所述加权参数进行调整。

9.根据权利要求6所述的破拆机器人的工作臂控制方法，其特征在于，通过视觉定位技术获取所述破拆目标点在工作臂坐标系下的第一坐标(A1)包括：利用视觉机器(1)获取所述破拆目标点在视觉坐标系下的视觉坐标(a1)，并根据所述视觉坐标系和所述工作臂坐标系之间的转换关系将所述视觉坐标(a1)转换为所述第一坐标(A1)。

10.根据权利要求6所述的破拆机器人的工作臂控制方法，其特征在于，通过激光定位技术获取所述破拆目标点在所述工作臂坐标系下的第二坐标(A2)包括：利用激光测距仪(2)获取所述破拆目标点在世界坐标系下的世界坐标(a2)，并根据所述世界坐标系和所述工作臂坐标系之间的转换关系将所述世界坐标(a2)转换为所述第二坐标(A2)。

11.一种破拆机器人的工作臂控制系统，其特征在于，包括：

工作臂(9)，具有用于执行破拆作业的工作臂末端(12)；

至少一个液压油缸(11)，与所述工作臂(9)连接以控制所述工作臂(9)运动；以及

控制器(5)，被配置为执行如下步骤：

获取所述工作臂(9)的工作臂末端(12)在当前状态下在工作臂坐标系下的末端坐标(B)；

获取破拆目标点在工作臂坐标系下的破拆坐标(C)；

根据所述末端坐标(B)和所述破拆坐标(C)获取使所述工作臂末端(12)移动至所述破拆目标点的运动路径，并基于多自由度机械臂的逆运动学解计算所述工作臂(9)在所述运动路径上的多个时间点处的多个工作臂姿态；

根据所述多个时间点处的多个工作臂姿态计算获取在多个时间点处所述至少一个液压油缸(11)的活塞杆的目标伸缩量；以及

根据所述多个时间点处的所述至少一个液压油缸(11)的活塞杆的目标伸缩量获取所述至少一个液压油缸(11)在多个时间点处的液压油的目标流量，并基于所述多个时间点处的液压油的目标流量控制所述至少一个液压油缸(11)动作。

12.根据权利要求11所述的破拆机器人的工作臂控制系统，其特征在于，所述控制器(5)还被配置为获取所述至少一个液压油缸(11)的进出流量与活塞杆的伸缩量之间的第一参数关系，并根据所述第一参数关系由所述至少一个液压油缸(11)的活塞杆的伸缩量获取所述至少一个液压油缸(11)在多个时间点处的液压油的流量。

13.根据权利要求12所述的破拆机器人的工作臂控制系统，其特征在于，所述破拆机器人还包括用于控制所述至少一个液压油缸(11)的进出流量的流量控制阀组，且所述流量控制阀组的阀口开度根据输入电流的大小变化，所述控制器(5)被配置为获取所述流量控制阀组在不同电流大小下的阀口开度，并推导出在不同阀口开度下通过所述阀口的液压油的流速，根据所述不同电流大小下的液压油的流速和所述多个时间点处的液压油的目标流量控制所述流量控制阀组的电流信号及开关时间。

14.根据权利要求13所述的破拆机器人的工作臂控制系统，其特征在于，所述流量控制阀组包括电磁比例阀(6)和多路阀(7)，所述多路阀(7)包括与所述至少一个液压油缸(11)对应连接的至少一个液压通道，所述至少一个液压通道的阀口开度根据所述电磁比例阀(6)的电流信号变化，所述控制器(5)被配置为控制所述电磁比例阀(6)的电流信号以控制所述至少一个液压通道的阀口开度。

15.根据权利要求13所述的破拆机器人的工作臂控制系统，其特征在于，所述控制器(5)被配置为在所述破拆机器人启动时，获取所述工作臂末端(12)在初始状态下的初始坐标(b)，且所述控制器(5)还被配置为获取所述工作臂末端(12)从所述初始状态到所述当前状态的过程中通过所述至少一个液压油缸(11)的实际流量，并根据所述通过至少一个液压油缸(11)的实际流量获取所述至少一个液压油缸(11)的活塞杆的实际伸缩量，然后通过所述初始坐标(b)和所述至少一个液压油缸(11)的活塞杆的实际伸缩量并基于多自由度机械臂的正运动学解推导出所述末端坐标(B)。

16.根据权利要求15所述的破拆机器人的工作臂控制系统，其特征在于，所述破拆机器人还包括回转马达(8)和由所述回转马达(8)驱动的回转平台(10)，所述工作臂(9)安装在所述回转平台(10)上，所述回转平台(10)用于带动所述工作臂(9)旋转，所述控制器(5)被配置为获取所述回转马达(8)的进出流量与所述回转平台(10)的旋转角度之间的第二参数关系，获取通过所述回转马达(8)的实际流量，通过所述初始坐标(b)、所述至少一个液压油缸(11)的活塞杆的实际伸缩量以及所述回转平台(10)的实际旋转角度并基于多自由度机械臂的正运动学解推导出所述末端坐标(B)。

17.根据权利要求11-16中任一项所述的破拆机器人的工作臂控制系统，其特征在于，所述破拆机器人还包括与所述控制器(5)电连接的复合定位装置，所述复合定位装置用于获取所述破拆目标点在视觉坐标系下的视觉坐标(a1)和在世界坐标系下的世界坐标(a2)并将坐标数据发送给所述控制器(5)，所述控制器(5)被配置为将所述视觉坐标(a1)和所述世界坐标(a2)转换为所述破拆坐标(C)。

18.根据权利要求17所述的破拆机器人的工作臂控制系统，其特征在于，所述复合定位装置包括二维云台(3)和设置在所述二维云台(3)上的视觉机器(1)及激光测距仪(2)，所述破拆机器人还包括光照传感器(4)，其中，

所述二维云台(3)可动地设置以使所述破拆目标点成像于所述视觉机器(1)的视场范围内且使所述激光测距仪(2)的光斑位于所述破拆目标点上，且所述二维云台(3)被配置为向所述控制器(5)发送俯仰角度数据和旋转角度数据；

所述视觉机器(1)被配置为获取所述破拆目标点在视觉坐标系下的视觉坐标(a1)且将所述视觉坐标(a1)发送至所述控制器(5)；

所述激光测距仪(2)被配置为获取所述破拆目标点与所述激光测距仪(2)之间的距离并向所述控制器(5)发送距离数据；

所述光照传感器(4)被配置为获取作业环境的光照亮度并将光照亮度发送至所述控制器(5)；

所述控制器(5)被配置为根据所述距离数据、所述俯仰角度数据和所述旋转角度数据计算得到所述破拆目标点在所述世界坐标系下的世界坐标(a2)，所述控制器(5)还被配置为将所述视觉坐标(a1)和所述世界坐标(a2)分别转换为工作臂坐标系下的第一坐标(A1)和第二坐标(A2)，并利用加权公式C＝kA1+(1-k)A2计算获取所述破拆坐标(C)，k为加权参数，所述加权参数根据光照亮度改变，当所述光照亮度在[0,10]之间时，所述加权参数为0，当所述光照亮度在[10,100]之间时，所述加权参数为0.25，当所述光照亮度在[100,500]之间时，所述加权参数为0.5，当所述光照亮度在[500,60000]之间时，所述加权参数为0.75，当所述光照亮度在[60000,200000]之间时，所述加权参数为1。

19.根据权利要求18所述的破拆机器人的工作臂控制系统，其特征在于，所述视觉机器(1)包括双目相机、单目相机、景深相机、结构光相机中的至少一种。

Images