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

n θ i-si η Θ θ 2氺sin Θ 3)_sin Θ 4^(cos θ 2氺sin Θ Θ 3+cos θ 3氺sin Θ θ 2));
[0049] T23= cos θ 1；
[0050] r31= -cos θ 5* (cos θ 4* (cos θ 2*sin θ 3+cos θ 3*sin θ 2)+sin θ 4* (cos θ 2*cos θ 2-s in θ 2木sin θ 3))_sx5木(cos θ 4>!<(cos θ 2木cos θ 3_sin θ 2木sin θ 3)_sin θ 4>!<(cos θ 2木sin θ 3+cos θ 3*sin θ 2));
[0051] r32= sin θ 5* (cos θ 4* (cos θ 2*sin θ 3+cos θ 2*sin θ 2)+sin θ 4* (cos θ 2*cos θ 3-s in θ 2氺sin θ 3)) -cos θ 5氺(cos θ 4氺(cos θ 2^cos θ 3_sin θ 2氺sin θ 3) -sin θ 4氺(cos θ 2氺sin θ 3+ cos θ 3*sin θ 2));
[0052] r33= 0 ；
[0053] ρχ= cos θ dLl-I^-sin θ fl^-sin θ fL^sin θ 1;
[0054] py= cos θ 4L2+COS θ 3L3+COS θ ^I^+Ll-sin θ 1;
[0055] Pz= 0 ；
[0056] 其中θ ρ θ 2、θ 3、θ 4、θ 5分别为关节1、关节2、关节3、关节4、关节5的关节角， LU L2、L3、L4分别为关节1与关节2、关节2与关节3、关节3与关节4、关节4与关节5的 直线距离。
[0057] 由齐次变换矩阵可得目标点在第一坐标系（关节1坐标系，即基坐标系）下的坐 标[X，y，z]为：
[0058] X = d* (cos Θ 5* (cos Θ 4* (cos Θ 2*cos Θ 3*sin Θ「sin Θ ,sin Θ 2*sin Θ 3) - sin Θ 4 氺(cos Θ 2氺sin Θ Θ 3+cos Θ 3氺sin Θ Θ 2)) -sin Θ 5氺(cos Θ 4氺(cos Θ 2氺sin Θ Θ 3 +cos Θ 3氺sin Θ Θ 2)+sin Θ 4氺(cos Θ 2^cos Θ 3氺sin Θ i-sin Θ Θ 2氺sin Θ 2)))；
[0059] y = _d* (cos Θ 5* (cos Θ 4* (cos Θ 2*sin Θ ,sin Θ 3+cos Θ 3*sin Θ ,sin Θ 2) +sin Θ 4* (cos Θ 2木cos Θ 3>!<sin Θ i-sin Θ i-sin Θ 2木sin Θ 3)) +sin Θ 5* (cos Θ 4木(cos Θ 2木cos Θ 3>!<sin Θ 丄一 sin Θ Θ 2氺sin Θ 3)-sin Θ 4氺(ccos Θ 2sin Θ Θ 3+cos Θ 3氺sin Θ Θ 2)))；
[0060] z = cos Θ ；
[0061] 通过运动学逆解将第一坐标系下的目标点坐标及设定的末端执行器位姿转换为5 关节液压驱动机械臂1的五个关节角Θ的角度值，设位姿值为nz，逆解方程过程如下：以 关节4为例，
[0062] Θ 4取关节角限定范围的任意一个值，可得
[0063] 得到
[0064] 设 ks3 = (x2+y2+z2_2*Ll* (x*cos Θ Ay-Sin Θ J+L12-L22-L32-L42_2*cos Θ 4*L3 礼4V(2*L2);
[0065] 设 rx = L4*sin Θ 4;
[0066] 设 ry = L3+L4*cos Θ 4;
[0067] 当满足rx2+ry2-ks32> 0时，可求出其他剩余关节角值：
[0068] 设
[0069] 得到
[0070] 设
[0071 ] s23 = - (L3*z+Ll* (L2*sin Θ 3-L4*sin Θ 4) -cos Θ # (L2*x*sin Θ 3-L4*x*sin Θ 4) -s in Θ 1*(L2*y*sin Θ 3-L4*y*sin Θ 4)+L2*z*cos Θ 3+L4*z*cos Θ 4)/(L12_2*L1*x*cos Θ f2*Ll 氺y氺sin Θ i+(x氺cos Θ i+y氺sin Θ ^2+z2)
[0072] C23 = - (Ll*L3_L2*z*sin Θ 3+L4*z*sin Θ 4-x*cos Θ 丨* (L3+L2*cos Θ 3+L4*cos Θ 4)- y*sin Θ ,(I^+I^^^cos Θ 3+L4*cos Θ 4)+L1*L2*cos Θ 3+L1*L4*cos Θ 4)/(L12_2*L1*x*cos Θ 厂2 *Ll*y*sin Θ j+(x>l< cos Θ j+y>l<sin Θ ^)2+z2)；
[0073] 得到
[0074] 得到 Θ 5= sin 1 (_nz) - θ 2- θ 3- Θ 4;
[0075] 对五个关节角的解进行验证，若满足各自的关节角限制条件，则可采取加权判断 来求出唯一最优解，加权判断公式如下：
[0076] 设机械臂初始姿态的各关节角为ΙΑΟ, IA1，IA2, IA3, IA4 ;各关节权重值总和为1， 分配在各关节的权重依次为W0，W1，W2，W3，W4 ;可得加权和sum = w0*| Θ fIA〇|+Wl*| Θ 2-1 Al|+W2*| 03-IA2|+W3*| 04-IA3|+W4*| Θ5-ΙΑ4| ;对于多个不同的加权和，取最小加权和的 各求解关节角作为逆解的最终解。
[0077] 与现有技术相比，本发明具有以下优点：
[0078] 1、开发一种破拆机器人手眼目标定位装置，可以有效满足机器人在恶劣环境下进 行精确作业定位的需要。
[0079] 2、设计依附于手眼目标定位装置的破拆机器人液压器件驱动多关节机械臂自主 运动控制算法解决了现有遥控器手工操作的动作迟缓及作业点找不准问题。
[0080] 对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在 不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论 从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权 利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有 变化囊括在本发明内。
[0081] 此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包 含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当 将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员 可以理解的其他实施方式。
【主权项】
1. 一种破拆机器人手眼协调定位装置，其特征在于，由多关节液压驱动机械臂、激光 测距传感器、CAN总线控制器、车载电脑构成；所述激光测距传感器安装在多关节液压驱动 机械臂的末端执行器上，所述激光测距传感器通过CAN总线控制器与车载电脑实现信息交 互。2. 根据权利要求1所述的破拆机器人手眼协调定位装置，其特征在于，所述激光测距 传感器采用的是SICK LMS151激光测距传感器。3.根据权利要求1所述的破拆机器人手眼协调定位装置，其特征在于，所述CAN总线控 制器采用的是PEAK-USB CAN总线控制器。4.根据权利要求1所述的破拆机器人手眼协调定位装置，其特征在于，所述车载电脑 采用的是TREK-753车载电脑。5. -种利用权利要求1-4任一所述的破拆机器人手眼协调定位装置进行定位的方法， 其特征在于，包括以下步骤： 1)激光测距传感器将采集的距离信息通过CAN总线发送到车载电脑； 2)车载电脑对接收到的距离信息进行处理后，再通过CAN总线发送液压元件的速度驱 动信号来驱使多关节液压驱动机械臂运动到指定目标位置，完成机械臂自动定位和运动控 制。6. 根据权利要求5所述的利用破拆机器人手眼协调定位装置进行定位的方法，其特征 在于，所述步骤2)中，车载电脑通过调用机械臂自动运动控制算法对接收到的距离信息进 行处理，具体包括以下步骤： 21) 将末端执行器上的激光测距传感器激光红点对准欲破拆的作业点； 22) 将获得的激光距离值通过齐次变换矩阵转为末端执行器下坐标； 23)通过连杆变换矩阵将末端执行器下坐标转为基坐标系下坐标； 24)根据逆运动学方程求出的各关节角角度； 25)通过对液压执行部件发送速度信号驱动机械臂运动。7.根据权利要求5-6任一所述的利用破拆机器人手眼协调定位装置进行定位的方法， 其特征在于，安装在多关节液压驱动机械臂的末端执行器上的激光测距传感器的激光发射 方法与末端执行器某一坐标轴方向一致。
【专利摘要】本发明公开了一种破拆机器人手眼协调定位装置及方法，所述装置由多关节液压驱动机械臂、激光测距传感器、CAN总线控制器、车载电脑构成；所述激光测距传感器安装在多关节液压驱动机械臂的末端执行器上，所述激光测距传感器通过CAN总线控制器与车载电脑实现信息交互；所述方法包括：1)激光测距传感器将采集的距离信息通过CAN总线发送到车载电脑；2)车载电脑对接收到的距离信息进行处理后，再通过CAN总线发送液压元件的速度驱动信号来驱使多关节液压驱动机械臂运动到指定目标位置，完成机械臂自动定位和运动控制。本发明有效提高了破拆机器人破拆作业的目标精度和作业效率。
【IPC分类】B25J5/06, B25J9/16
【公开号】CN105215972
【申请号】CN201510594452
【发明人】赵江海, 何锋, 章小建, 李岩峰, 方健, 朱洪波
【申请人】中国科学院合肥物质科学研究院
【公开日】2016年1月6日
【申请日】2015年9月18日