机械臂姿态检测方法和装置.pdf

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN114102581A(43)申请公布日2022.03.01(21)申请号202111266685.0(22)申请日2021.10.28(71)申请人珠海格力电器股份有限公司地址519070广东省珠海市前山金鸡西路(72)发明人李俊林郑毅锋张锋(74)专利代理机构广州市时代知识产权代理事务所(普通合伙)44438代理人刁益帆(51)Int.Cl.B25J9/16(2006.01)B25J9/00(2006.01)权利要求书3页说明书10页附图4页(54)发明名称机械臂姿态检测方法和装置(57)摘要本申请是关于一种机械臂姿态检测方法和装置。该方法包括以下步骤：获取机械臂上N个动态检测点在Ti时刻的N个检测位置坐标；所述N为大于或等于一的整数；其中，一个动态检测点在Ti时刻的检测位置坐标根据两个蓝牙模块分别在两个静态接收点的检测数据计算得到；获取所述机械臂在所述Ti时刻的N个预设位置坐标；根据N个所述检测位置坐标和N个所述预设位置坐标确定所述机械臂是否出现姿态偏差。本申请根据同一动态检测点在示教器训练时记录的Ti时刻的预设位置坐标和该动态检测点在Ti时刻的检测位置坐标进行比对，进而确认该动态检测点是否偏离了预设位置坐标，即机械臂是否出现姿态偏差，进而防止该机械臂在进行自动化操作时出现错误操作。CN114102581ACN114102581A权利要求书1/3页1.一种机械臂姿态检测方法，其特征在于，包括：获取机械臂上N个动态检测点在Ti时刻的N个检测位置坐标；所述N为大于或等于一的整数；其中，一个动态检测点在Ti时刻的检测位置坐标根据两个蓝牙模块分别在两个静态接收点的检测数据计算得到；获取所述机械臂在所述Ti时刻的N个预设位置坐标；根据N个所述检测位置坐标和N个所述预设位置坐标确定所述机械臂是否出现姿态偏差。2.根据权利要求1所述的一种机械臂姿态检测方法，其特征在于，所述一个动态检测点在Ti时刻的检测位置坐标的计算方式，包括：获取一个动态检测点在Ti时刻的第一相位差和第二相位差，所述第一相位差为第一蓝牙模块在第一静态接收点对动态检测点的相位检测值，所述第二相位差为第二蓝牙模块在第二静态接收点对动态检测点的相位检测值；根据第一相位差和第二相位差分别获取所述动态检测点的第一到达角和第二到达角；根据第一到达角和第二到达角获取所述动态检测点在Ti时刻的检测位置坐标。3.根据权利要求1所述的一种机械臂姿态检测方法，其特征在于，所述获取机械臂上N个动态检测点在Ti时刻的N个检测位置坐标之后，包括：将N个所述检测位置坐标拟合为所述机械臂在Ti时刻的实际运动姿态；所述获取所述机械臂在所述Ti时刻的N个预设位置坐标之后，包括：将N个预设位置坐标拟合为所述机械臂在Ti时刻的预设运动姿态；所述根据N个所述检测位置坐标和N个所述预设位置坐标确定所述机械臂是否出现姿态偏差，包括：比对所述机械臂在Ti时刻的实际运动姿态和预设运动姿态，若运动姿态不一致，则确认所述机械臂出现姿态偏差。4.根据权利要求1所述的一种机械臂姿态检测方法，其特征在于，所述根据N个所述检测位置坐标和N个所述预设位置坐标确定所述机械臂是否出现姿态偏差，包括：根据一个所述动态检测点的偏差值确定是否存在姿态偏差；所述偏差值为所述动态检测点的所述检测位置坐标与所述预设位置坐标的距离；若所述偏差值大于预设阈值，则发送停机指令和报警指令；若所述偏差值小于预设阈值，则计算下一个所述动态检测点的偏差值。5.根据权利要求2所述的一种机械臂姿态检测方法，其特征在于，所述根据第一相位差和第二相位差分别获取所述动态检测点的第一到达角和第二到达角，包括：根据所述第一相位差、所述第二相位差和到达角计算公式得到所述第一静态接收点和所述动态检测点的到达角θ1、所述第二静态接收点和所述动态检测点的到达角θ2；所述到达角计算公式为：θ＝arccos((ψλ)/2πd)；所述θ为到达角，所述ψ为相位差；所述λ为波长；所述d为所述静态接收点两组蓝牙天线的预设距离。6.根据权利要求2所述的一种机械臂姿态检测方法，其特征在于，所述根据第一到达角和第二到达角获取所述动态检测点在Ti时刻的检测位置坐标，包2CN114102581A权利要求书2/3页括：根据所述第一到达角、所述第二到达角和三角定位公式得到所述动态检测点在Ti时刻的检测位置坐标；所述三角定位公式为：xi＝((y2‑x2tanθ2)‑(y1‑x1tanθ1))/(tanθ2‑tanθ1)；yi＝((y2‑x2cotθ2)‑(y1‑x1cotθ1))/(cotθ2‑cotθ1)；(x1，y1)为所述第一静态接收点的坐标；(x2，y2)为所述第二静态接收点的坐标；(xi，yi)为所述动态检测点在Ti时刻的