CN105215972A - 一种破拆机器人手眼协调定位装置及方法 - Google Patents

Abstract

本发明公开了一种破拆机器人手眼协调定位装置及方法，所述装置由多关节液压驱动机械臂、激光测距传感器、CAN总线控制器、车载电脑构成；所述激光测距传感器安装在多关节液压驱动机械臂的末端执行器上，所述激光测距传感器通过CAN总线控制器与车载电脑实现信息交互；所述方法包括：1)激光测距传感器将采集的距离信息通过CAN总线发送到车载电脑；2)车载电脑对接收到的距离信息进行处理后，再通过CAN总线发送液压元件的速度驱动信号来驱使多关节液压驱动机械臂运动到指定目标位置，完成机械臂自动定位和运动控制。本发明有效提高了破拆机器人破拆作业的目标精度和作业效率。

Description

一种破拆机器人手眼协调定位装置及方法

技术领域

本发明涉及机器人定位技术，具体是一种破拆机器人手眼协调定位装置及方法。

背景技术

在面临地震、泥石流等灾害时，需要紧急进行救援来尽量降低人员及财产损失，因此提高震后的应急救援能力极其重要，用于灾害现场的安全应急救灾设备也是“十二五”时期中国机械工业的发展重点之一。

日本的东芝、三菱公司等相继开展了用于危险现场搬运作业的机器人，如东芝公司研发的Smart-M机器人可以搬运5kg左右的重物，三菱公司研制的Swan机器人具有地形适应能力，并可以搬运10kg左右的中等载荷。但是这些破拆机器人一般均采用遥控器人工手工操作方式进行破拆作业，而在现场复杂和危险的环境下，对操作员人身安全带来很大威胁。针对文献“破拆救援机器人关键技术”(刘庆运.《现代制造工程》，2009，30(7)，pp.149～153.)中提出未来破拆机器人具备的功能有智能破拆作业，实时运动规划，传感与目标识别等关键技术，本发明提出的装置及方法能解决传统的破拆机器人存在的作业目标定位及机械臂自主运动功能，填补破拆机器人智能破拆作业功能应用的空白。

发明内容

本发明的目的在于提供一种破拆机器人手眼协调定位装置及方法，解决了破拆机器人作业目标定位问题。

为实现上述目的，本发明提供如下技术方案：

一种破拆机器人手眼协调定位装置，由多关节液压驱动机械臂、激光测距传感器、CAN总线控制器、车载电脑构成；所述激光测距传感器安装在多关节液压驱动机械臂的末端执行器上，所述激光测距传感器通过CAN总线控制器与车载电脑实现信息交互。

作为本发明进一步的方案：所述激光测距传感器采用的是SICKLMS151激光测距传感器。

作为本发明进一步的方案：所述CAN总线控制器采用的是PEAK-USBCAN总线控制器。

作为本发明进一步的方案：所述车载电脑采用的是TREK-753车载电脑。

一种利用所述的破拆机器人手眼协调定位装置进行定位的方法，包括以下步骤：

1)激光测距传感器将采集的距离信息通过CAN总线发送到车载电脑；

2)车载电脑对接收到的距离信息进行处理后，再通过CAN总线发送液压元件的速度驱动信号来驱使多关节液压驱动机械臂运动到指定目标位置，完成机械臂自动定位和运动控制。

作为本发明进一步的方案：所述步骤2)中，车载电脑通过调用机械臂自动运动控制算法对接收到的距离信息进行处理，具体包括以下步骤：

21)将末端执行器上的激光测距传感器激光红点对准欲破拆的作业点；

22)将获得的激光距离值通过齐次变换矩阵转为末端执行器下坐标；

23)通过连杆变换矩阵将末端执行器下坐标转为基坐标系下坐标；

24)根据逆运动学方程求出的各关节角角度；

25)通过对液压执行部件发送速度信号驱动机械臂运动。

作为本发明进一步的方案：安装在多关节液压驱动机械臂的末端执行器上的激光测距传感器的激光发射方法与末端执行器某一坐标轴方向一致。

与现有技术相比，本发明的有益效果是：

1、开发一种破拆机器人手眼目标定位装置，可以有效满足机器人在恶劣环境下进行精确作业定位的需要。

2、设计依附于手眼目标定位装置的破拆机器人液压器件驱动多关节机械臂自主运动控制算法解决了现有遥控器手工操作的动作迟缓及作业点找不准问题。

附图说明

图1是本发明定位装置的结构示意图；

图2是实施例的示意图；

图3为破拆机器人多关节机械臂连杆坐标系布局图；

图4是本发明定位方法的流程图；

图5为机械臂自动运动控制算法的流程图；

图中：1-多关节液压驱动机械臂、2-激光测距传感器、3-CAN总线控制器、4-车载电脑。

具体实施方式

下面将结合本发明实施例，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅图1～2，本发明实施例中，一种破拆机器人手眼协调定位装置，由多关节液压驱动机械臂1、激光测距传感器2、CAN总线控制器3、车载电脑4构成；激光测距传感器2安装在多关节液压驱动机械臂1的末端执行器上，其激光发射方法与末端执行器某一坐标轴方向一致，激光测距传感器2将采集的距离信息通过CAN总线发送到车载电脑，由车载电脑进一步调用机械臂自主运动控制算法进行下一步处理，最后由车载电脑4通过CAN总线发送液压元件的速度驱动信号来驱使多关节液压驱动机械臂1运动到指定目标位置，完成机械臂自动定位和运动控制。定位装置的控制原理如下：通过破拆机器人自带云台摄像机，观察到现场的画面，通过远端控制器控制末端执行器对准破拆目标，激光测距传感器2获得破拆目标的距离并通过CAN总线传回车载电脑4，车载电脑4将激光距离信息、机械臂当前姿态、液压缸有效运动范围等数据代入机械臂自主运动控制算法，驱动机械臂进行运动。

多关节液压驱动机械臂1采用的是5关节液压驱动机械臂，激光测距传感器2采用的是SICKLMS151激光测距传感器，CAN总线控制器3采用的是PEAK-USBCAN总线控制器，车载电脑4采用的是TREK-753车载电脑。

请参阅图4，一种利用所述的破拆机器人手眼协调定位装置进行定位的方法，包括以下步骤：

1)激光测距传感器(2)将采集的距离信息通过CAN总线发送到车载电脑(4)；

2)车载电脑(4)对接收到的距离信息进行处理后，再通过CAN总线发送液压元件的速度驱动信号来驱使多关节液压驱动机械臂(1)运动到指定目标位置，完成机械臂自动定位和运动控制。

上述步骤2)中，车载电脑(4)通过调用机械臂自动运动控制算法对接收到的距离信息进行处理，具体包括以下步骤：

21)将末端执行器上的激光测距传感器(2)激光红点对准欲破拆的作业点；

22)将获得的激光距离值通过齐次变换矩阵转为末端执行器下坐标；

23)通过连杆变换矩阵将末端执行器下坐标转为基坐标系下坐标；

24)根据逆运动学方程求出的各关节角角度；

25)通过对液压执行部件发送速度信号驱动机械臂运动。

请参阅图3和5，机械臂自主运动控制算法，步骤为：

算法步骤为：

1、根据定位装置测出的激光测距传感器2在末端执行器安装位置之间的转换矩阵将作业目标距离转为末端执行器坐标系下三维坐标。激光测距传感器2安装在末端执行器坐标系的Y轴原点，激光器发射方向与Y轴正向一致，当前激光测距传感器2的读数是d米，则作业目标在第五坐标系(关节5坐标系，即末端执行器坐标系)下的坐标表示则为[0，d，0]。

2、通过基坐标系与末端执行器坐标系之间的齐次转换矩阵计算获取激光测距***下的目标物体坐标。末端执行器坐标系(关节5坐标系)相对基坐标系(关节1坐标系)的齐次变换矩阵为 $\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& p_x\\ r_{21}& r_{22}& r_{23}& p_y\\ r_{31}& r_{32}& r_{33}& P_z\\ 0& 0& 0& 1\end{array}\right],$ 其中r₁₁＝cosθ₅*(cosθ₄*(cosθ₁*cosθ₂*cosθ₃-cosθ₁′*sinθ₂*sinθ₃)-sinθ₄*(cosθ₁*cosθ₂*sinθ₃+cosθ₁*cosθ₃*sinθ₂))-sinθ₅*(cosθ₄*(cosθ₁*cosθ₂*sinθ₃+cosθ₁*cosθ₃*sinθ₂)+sinθ₄*(cosθ₁*cosθ₂*cosθ₃-cosθ₁*sinθ₂*sinθ₃))；

r₁₂＝-cosθ₅*(cosθ₄*(cosθ₁*cosθ₂*sinθ₃+cosθ₁*cosθ₃*sinθ₂)+sinθ₄*(cosθ₁*cosθ₂*cosθ₃-cosθ₁*sinθ₂*sinθ₃))-sinθ₅*(cosθ₄*(cosθ₁*cosθ₂*cosθ₃-cosθ₁*sinθ₂*sinθ₃)-sinθ₄*(cosθ₁*cosθ₂*sinθ₃+cosθ₁*cosθ₃*sinθ₂))；

r₁₃＝-sinθ₁；

r₂₁＝cosθ₅*(cosθ₄*(cosθ₂*cosθ₃*sinθ₁-sinθ₁*sinθ₂*sinθ₃)-sinθ₄*(cosθ₂*sinθ₁*sinθ₃+cosθ₃*sinθ₁*sinθ₂))-sinθ₅*(cosθ₄*(cosθ₂*sinθ₁*sinθ₃+cosθ₃*sinθ₁*sinθ₂)+sinθ₄*(cosθ₂*cosθ₃*sinθ₁-sinθ₁*sinθ₂*sinθ₃))；

r₂₂＝-cosθ₅*(cosθ₄*(cosθ₂*sinθ₁*sinθ₃+cosθ₃*sinθ₁*sinθ₂)+sinθ₄*(cosθ₂*cosθ₃*sinθ₁-sinθ₁*sinθ₂*sinθ₃))-sinθ₅*(cosθ₄*(cosθ₂*cosθ₃*sinθ₁-sinθ₁*sinθ₂*sinθ₃)-sinθ₄*(cosθ₂*sinθ₁*sinθ₃+cosθ₃*sinθ₁*sinθ₂))；

r₂₃＝cosθ₁；

r₃₁＝-cosθ₅*(cosθ₄*(cosθ₂*sinθ₃+cosθ₃*sinθ₂)+sinθ₄*(cosθ₂*cosθ₂-sinθ₂*sinθ₃))-sx5*(cosθ₄*(cosθ₂*cosθ₃-sinθ₂*sinθ₃)-sinθ₄*(cosθ₂*sinθ₃+cosθ₃*sinθ₂))；

r₃₂＝sinθ₅*(cosθ₄*(cosθ₂*sinθ₃+cosθ₂*sinθ₂)+sinθ₄*(cosθ₂*cosθ₃-sinθ₂*sinθ₃))-cosθ₅*(cosθ₄*(cosθ₂*cosθ₃-sinθ₂*sinθ₃)-sinθ₄*(cosθ₂*sinθ₃+cosθ₃*sinθ₂))；

r₃₃＝0；

p_x＝cosθ₁*L1-L2*sinθ₁-L3*sinθ₁-L4*sinθ₁；

p_y＝cosθ₁*L2+cosθ₁*L3+cosθ₁*L4+L1*sinθ₁；

p_z＝0；

其中θ₁、θ₂、θ₃、θ₄、θ₅分别为关节1、关节2、关节3、关节4、关节5的关节角，L1、L2、L3、L4分别为关节1与关节2、关节2与关节3、关节3与关节4、关节4与关节5的直线距离。

由齐次变换矩阵可得目标点在第一坐标系(关节1坐标系，即基坐标系)下的坐标[x，y，z]为：

x＝d*(cosθ₅*(cosθ₄*(cosθ₂*cosθ₃*sinθ₁-sinθ₁*sinθ₂*sinθ₃)-sinθ₄*(cosθ₂*sinθ₁*sinθ₃+cosθ₃*sinθ₁*sinθ₂))-sinθ₅*(cosθ₄*(cosθ₂*sinθ₁*sinθ₃+cosθ₃*sinθ₁*sinθ₂)+sinθ₄*(cosθ₂*cosθ₃*sinθ₁-sinθ₁*sinθ₂*sinθ₂)))；

y＝-d*(cosθ₅*(cosθ₄*(cosθ₂*sinθ₁*sinθ₃+cosθ₃*sinθ₁*sinθ₂)+sinθ₄*(cosθ₂*cosθ₃*sinθ₁-sinθ₁*sinθ₂*sinθ₃))+sinθ₅*(cosθ₄*(cosθ₂*cosθ₃*sinθ₁-sinθ₁*sinθ₂*sinθ₃)-sinθ₄*(ccosθ₂sinθ₁*sinθ₃+cosθ₃*sinθ₁*sinθ₂)))；

z＝cosθ₁*d；

通过运动学逆解将第一坐标系下的目标点坐标及设定的末端执行器位姿转换为5关节液压驱动机械臂1的五个关节角θ的角度值，设位姿值为nz，逆解方程过程如下：以关节4为例，

θ₄取关节角限定范围的任意一个值，可得

得到 ${\mathrm{\θ}}_1={\tan }^{-1}\frac{y}{x};$

设ks3＝(x²+y²+z²-2*L1*(x*cosθ₁+y*sinθ₁)+L1²-L2²-L3²-L4²-2*cosθ₄*L3*L4)/(2*L2)；

设rx＝L4*sinθ₄；

设ry＝L3+L4*cosθ₄；

当满足rx²+ry²-ks3²≥0时，可求出其他剩余关节角值：

设 $td=\sqrt{x^2+y^2-ks{3}^2};$

得到 ${\mathrm{\θ}}_3={\tan }^{-1}\frac{y}{x}-{\tan }^{-1}\frac{ky3}{td};$

设

s23＝-(L3*z+L1*(L2*sinθ₃-L4*sinθ₄)-cosθ₁*(L2*x*sinθ₃-L4*x*sinθ₄)-sinθ₁*(L2*y*sinθ₃-L4*y*sinθ₄)+L2*z*cosθ₃+L4*z*cosθ₄)/(L1²-2*L1*x*cosθ₁-2*L1*y*sinθ₁+(x*cosθ₁+y*sinθ₁)²+z²)

C23＝-(L1*L3-L2*z*sinθ₃+L4*z*sinθ₄-x*cosθ₁*(L3+L2*cosθ₃+L4*cosθ₄)-y*sinθ₁*(L3+L2*cosθ₃+^L4*cosθ₄)+L1*L2*cosθ₃+L1*L4*cosθ₄)/(L1²-2*L1*x*cosθ₁-2*L1*y*sinθ₁+(x*cosθ₁+y*sinθ₁)²+z²)；

得到 ${\mathrm{\θ}}_2={\tan }^{-1}\frac{s23}{c23};$

得到θ₅＝sin^-1(-nz)-θ₂-θ₃-θ₄；

对五个关节角的解进行验证，若满足各自的关节角限制条件，则可采取加权判断来求出唯一最优解，加权判断公式如下：

设机械臂初始姿态的各关节角为IA0，IA1，IA2，IA3，IA4；各关节权重值总和为1，分配在各关节的权重依次为W0，W1，W2，W3，W4；可得加权和sum＝w0*|θ₁-IA0|+W1*|θ₂-IA1|+W2*|θ₃-IA2|+W3*|θ₄-IA3|+W4*|θ₅-IA4|；对于多个不同的加权和，取最小加权和的各求解关节角作为逆解的最终解。

与现有技术相比，本发明具有以下优点：

1、开发一种破拆机器人手眼目标定位装置，可以有效满足机器人在恶劣环境下进行精确作业定位的需要。

2、设计依附于手眼目标定位装置的破拆机器人液压器件驱动多关节机械臂自主运动控制算法解决了现有遥控器手工操作的动作迟缓及作业点找不准问题。

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。

此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。

Claims (7)

1.一种破拆机器人手眼协调定位装置，其特征在于，由多关节液压驱动机械臂、激光测距传感器、CAN总线控制器、车载电脑构成；所述激光测距传感器安装在多关节液压驱动机械臂的末端执行器上，所述激光测距传感器通过CAN总线控制器与车载电脑实现信息交互。

2.根据权利要求1所述的破拆机器人手眼协调定位装置，其特征在于，所述激光测距传感器采用的是SICKLMS151激光测距传感器。

3.根据权利要求1所述的破拆机器人手眼协调定位装置，其特征在于，所述CAN总线控制器采用的是PEAK-USBCAN总线控制器。

4.根据权利要求1所述的破拆机器人手眼协调定位装置，其特征在于，所述车载电脑采用的是TREK-753车载电脑。

5.一种利用权利要求1-4任一所述的破拆机器人手眼协调定位装置进行定位的方法，其特征在于，包括以下步骤：

1)激光测距传感器将采集的距离信息通过CAN总线发送到车载电脑；

2)车载电脑对接收到的距离信息进行处理后，再通过CAN总线发送液压元件的速度驱动信号来驱使多关节液压驱动机械臂运动到指定目标位置，完成机械臂自动定位和运动控制。

6.根据权利要求5所述的利用破拆机器人手眼协调定位装置进行定位的方法，其特征在于，所述步骤2)中，车载电脑通过调用机械臂自动运动控制算法对接收到的距离信息进行处理，具体包括以下步骤：

21)将末端执行器上的激光测距传感器激光红点对准欲破拆的作业点；

22)将获得的激光距离值通过齐次变换矩阵转为末端执行器下坐标；

23)通过连杆变换矩阵将末端执行器下坐标转为基坐标系下坐标；

24)根据逆运动学方程求出的各关节角角度；

25)通过对液压执行部件发送速度信号驱动机械臂运动。

7.根据权利要求5-6任一所述的利用破拆机器人手眼协调定位装置进行定位的方法，其特征在于，安装在多关节液压驱动机械臂的末端执行器上的激光测距传感器的激光发射方法与末端执行器某一坐标轴方向一致。