一种双串联机械臂式移动操作机装置的碰撞检测方法
【技术领域】
[0001] 本发明涉及一种碰撞检测方法，尤其涉及一种双串联机械臂式移动操作机装置的 碰撞检测方法。
【背景技术】
[0002] 在专利[CN201510124392]中，提出了一种双串联机械臂式移动操作机装置的自 动化定位方法，但在实际工作中，由于双串联机械臂式移动操作机装置的串联式机械臂处 在动态的、不确定的非结构性环境中，因此碰撞现象可能经常发生，所以如何判断和避免串 联式机械臂的碰撞对于双串联机械臂式移动操作机装置的实际应用非常重要，然而未见到 针对双串联机械臂式移动操作机装置的系统化的碰撞检测方法。
【发明内容】
[0003] 基于以上目的，本发明提出一种双串联机械臂式移动操作机装置的碰撞检测方 法，具体方法如下： 首先，针对双串联机械臂式移动操作机装置的工作情况进行碰撞检测理论分析，利用 机器人运动学理论知识，建立双串联机械臂式移动操作机装置的运动学坐标系以及用户坐 标系，建立坐标变换矩阵，导出各个关节中心点的坐标表达式，求得任意两连杆之间的最 小距离表达式，采用包络半径法对两连杆之间的最小距离与两连杆的包络半径之和进行比 较，得到碰撞检测方程。
[0004] 接着，针对单机械臂作业的情况进行分析，利用Matlab工具，结合蒙特卡罗法获 取单机械臂作业可能发生碰撞的情况。
[0005] 然后，针对双机械臂同时工作的情况进行分析，利用Matlab工具，结合蒙特卡罗 法获取双机械臂同时作业可能发生碰撞的情况。
[0006] 再后，针对双串联机械臂式移动操作机装置的两个串联式机械臂末端的目标位 姿，利用Matlab工具以及双串联机械臂式移动操作机装置的解析逆解表达式获取两个串 联式机械臂的各个关节变量，其中€为第1个运动自由度，实现机械臂的摆动；奪为第2个 自由度，实现机械臂的俯仰为第3个自由度，实现机械臂的回转；_为第4个自由度，实 现滑架俯仰为第5个自由度，实现滑架摆动；為:为第6个自由度，实现滑架的移动。
[0007] 最后，单机械臂作业时，针对蒙特卡罗法检测出来的单机械臂各连杆之间可能发 生的碰撞情况进行单臂碰撞检测；双机械臂同时作业时，针对蒙特卡罗法检测出来的两个 串联式机械臂各连杆之间可能发生的碰撞情况进行双机械臂碰撞检测。
[0008] 本发明的碰撞检测方法的有益效果是：针对双串联机械臂式移动操作机装置提出 了单机械臂作业及双机械臂同时作业时碰撞检测的系统方法，得到了检测连杆碰撞的检测 方程，分析了单机械臂和双机械臂可能发生的碰撞情况，只对可能发生碰撞的情况进行碰 撞检测，缩短了检测时间，为实现双串联机械臂式移动操作机装置的实时碰撞检测奠定了 基础。
【附图说明】
[0009] 图1是本发明的双串联机械臂式移动操作机装置的结构示意图； 图2是本发明的双串联机械臂式移动操作机装置的运动学坐标系和用户坐标系； 图3是本发明的碰撞检测方法的流程图。 具体实施例
[0010] 本发明方法适用于图1所示的双串联机械臂式移动操作机装置，该装置由车身和 两个串联式机械臂组成，两个串联式机械臂安装在车身的前部上面左右对称布置，定位的 执行部件为两个串联式机械臂，串联式机械臂包括：滑块1，末端操作器2,滑架3,推进器4， 整臂摆动油缸5,升降油缸6,机械臂7,俯仰油缸8和摆角油缸9。
[0011] 机械臂7的固定部分和车身铰接，机械臂7的回转部分和滑架3铰接。升降油缸 6布置在机械臂7的下面后侧，升降油缸6的后侧和整臂摆动油缸5铰接、升降油缸6的前 侧和机械臂7的固定部分铰接。俯仰油缸8布置在机械臂7的下面前侧，俯仰油缸8的后 侧和机械臂7的回转部分铰接，俯仰油缸8的前侧和滑架3铰接。摆角油缸9布置在机械 臂7的前侧，摆角油缸9的后侧和机械臂7的回转部分铰接，摆角油缸9的前侧和滑架3铰 接。滑架3布置在机械臂7的上部，滑架3的后侧和机械臂7的回转部分铰接，滑架3的前 侧和摆角油缸9的前侧铰接。推进器4布置在滑架3的上部。末端操作器2布置在推进器 4的上部和推进器4形成移动副。滑块1布置滑架3上部。左侧和右侧的串联式机械臂结 构基本相同，两个串联式机械臂的区别在于：左侧的串联式机械臂（简称左臂）的整臂摆动 油缸（5)布置在机械臂左侧，右侧的串联式机械臂(简称右臂）的整臂摆动油缸（5)布置在 机械臂右侧。
[0012] 针对如图1所示的双串联机械臂式移动操作机装置，碰撞检测的过程如下所示： 第一步，碰撞检测方程求取 建立双串联机械臂式移动操作机装置的运动学坐标系和用户坐标系如图2所示。考 虑到研究的双串联机械臂式移动操作机装置为双机械臂操作，且两个机械结构基本一致， 两个串联式机械臂的D-Η参数中的常量参数完全一致。以某实际装置为例，根据建立的坐 标系确定D-Η参数如表1所示，分析方位初始角为，琴=:Θ气乌=，爲=〇°，巧=， ：~ -90° 0
[0013] 表1串联式机械臂的各连杆D-Η参数
用户坐标系到串联式机械臂的J变换矩阵，其中用户坐标系到左臂的变换式中:巧<〇， 用户坐标系到右臂的变换式中，<〇。
[0014] 利用图2的运动学坐标系以及坐标变换矩阵4-4，导出每个连杆的两端的关节 中心点坐标。
[0015] 设定用户坐标系原点为：，则基坐标系原点为：
[0016] 第1个坐标系原点为：Q= (1)
将坐标系原点4对应的点记为点4，将点讀与点1?.之间的连杆记作为4j+i。考虑 连杆L+1与连杆的碰撞问题，将两连杆上任意两点之间的距离记作为，利用 的最小值来判断两连杆之间是否发生碰撞。连杆|_与连杆(^_+1所在直线上的点可 分别表不为：
通过对式（7)的分析，结合空间两条直线之间的关系，将单机械臂运动关节的碰撞检测 分两种情况讨论：连杆b+1和匕/+1所在的两条直线的公垂线的垂足均在连杆上；连杆b+1 和所在的两条直线的公垂线的垂足不都在连杆上。
[0017] (1)当连杆心+1和幻+1所在的两条直线的公垂线的垂足均在连杆上时，说明 0 且的最小值则为两直线的公垂线的大小。两直线的公垂线的大小 ~(^V)的求法是将ω和^求出然后带入公式（7)即可。
[0018] 下面计算求解:轉和分。将&^0平方，两端再分别对·和欠求偏导并令其为零得：
解得：
若两直线的公垂线的大小&^4 >25 (j为连杆的包络半径，此处取两个连杆的包络 半径相等)，两个连杆之间不发生碰撞，否则两连杆之间发生碰撞。
[0019] (2)当连杆。+1和连杆所在的两条直线的公垂线的垂足不都在连杆上时，这 时⑷,V)e(0,1)X(0,1)，两直线距离函数'ιΧV)2在区域(〇, 1)X(〇，1)内没有极值点，极值点只 能在[αι]χ[0,ι]的边界上达到，即在区间（cu)x(cu)内，两直线之间距离没有最值，最值只能 在边界上取得。那么，判断两连杆是否发生碰撞的条件（即两连杆之间的最小距离% :::)要 考虑多种情况。
[0020] 对_和，分别讨论，求取两连杆之间的最小距离，最后得出&kv；)在区域 [0,1]χ[0,1]的边界上的最小值为：
若> 25，两个连杆不会发生碰撞，否则两连杆发生碰撞。
[0021 ] 碰撞检测方法的具体流程如图3所示。
[0022] 第二步，单机械臂作业时可能发生的碰撞情况分析 单机械臂作业时，利用Matlab工具，结合蒙特卡罗法，将碰撞检测方程编写成Matlab程序，通过观察串联式机械臂结构，相邻连杆之间不可能发生碰撞，所以单机械臂作业时只 考虑不相邻连杆之间的碰撞。在碰撞方程的Matlab程序中应用循环语句，将单机械臂的所 有不相邻的连杆之间的碰撞情况编写在内，随机的给定2000个点，对所有不相邻连杆之间 的碰撞情况进行检测，输出结果显示具体串联式机械臂的哪两个连杆之间有无碰撞。具体 步骤如下： 1、 在各关节变量的取值范围内（见表1 )，对每个关节变量取一定的步长值，从而可得到 _(截=;'._:_:)组变量值的组合； 2、 求解串联式机械臂的碰撞检测方程，确定串联式机械臂两连杆间最小距离距离方程 (见式（8)与式（9)); 3、 将步骤1中生成的λ组变量值代入所求得的碰撞检测方程程序，得到和个方程解的 值，将其对