[发明专利]机械臂姿态检测方法和装置

权利要求书

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所述的一种机械臂姿态检测方法，其特征在于，

所述根据第一相位差和第二相位差分别获取所述动态检测点的第一到达角和第二到达角，包括：

根据所述第一相位差、所述第二相位差和到达角计算公式得到所述第一静态接收点和所述动态检测点的到达角θ₁、所述第二静态接收点和所述动态检测点的到达角θ₂；

所述到达角计算公式为：θ＝arccos((ψλ)/2πd)；所述θ为到达角，所述ψ为相位差；所述λ为波长；所述d为所述静态接收点两组蓝牙天线的预设距离。