[发明专利]一种基于UR机器人的UWB基站坐标自动标定方法

权利要求书

1.一种基于UR机器人的UWB基站坐标自动标定方法，其特征是按如下步骤进行：

步骤1、构建UWB基站坐标自动标定系统：

步骤1.1、在UWB定位区域内，任意选择三个位置作为三个定位基站的摆放点，并使得三个定位基站处于同一水平面；

步骤1.2、将UR机器人放置在三个定位基站之间的区域内；将定位标签T放置在UR机器人的机械臂上，并使得三个定位基站和定位标签T处在同一水平面；由所述定位标签T和三个定位基站构成UWB定位系统；

步骤1.3、UR机器人驱动机械臂携带所述定位标签T在三个定位基站所处的水平面内做有规律地平面运动；

步骤2、数据发送和接受：

步骤2.1、UR机器人记录不同时刻所述定位标签T在UR机器人坐标系下的坐标值；并固定采样间隔为Δt，将K个不同时刻下所述定位标签T的坐标记为T₁(x_t1,y_t1),T₂(x_t2y_t2),...,T_k(x_tk,y_tk),...,T_K(x_tK,y_tK)并通过以太网传输到PC端，其中，T_k(x_tk,y_tk)表示第k个时刻所述定位标签T的坐标；

步骤2.2、当UWB定位系统进行通讯测距时，选择任意一个定位基站作为主基站base，其他基站依次为第一次基站sub1，第二次基站sub2；固定采样间隔Δt，将K个不同时刻下各个基站与所述定位标签T的测距值发送至主基站上，由所述主基站统一通过数据线传输到PC端；

步骤3、初步估计定位基站的坐标：

步骤3.1、利用式(1)计算圆周定位模型：

式(1)中，x_tk和y_tk表示所述定位标签T在第k个时刻T_k的横、纵坐标值；x_i和y_i表示任意第i个定位基站的横、纵坐标值；di_tk表示所述定位标签T在第k个时刻定位标签T与第i个定位基站的距离值；base表示主基站，sub1表示第一次基站1，sub2表示第一次基站2；

步骤3.2、分别将第k时刻定位标签T的坐标值以及第k时刻第i个定位基站与定位标签T的距离值带入式(1)中并联立方程组；

步骤3.3、使用最小二乘法公式计算方程组的最优解，得到UR机器人坐标系下第i个定位基站的坐标(x_i,y_i)；

步骤4、UWB定位系统基站标定；

步骤4.1、计算坐标系变换矩阵；

将UR机器人的坐标系设为ω坐标系，将UWB定位系统的坐标系设为b坐标系；UWB定位系统的b坐标系以主基站为原点，其x轴和y轴分别与UR机器人的w坐标系的x轴和y轴平行同向；

利用式(2)得到ω坐标系变换到b坐标系的变换矩阵

式(2)中，R表示旋转矩阵，t表示平移向量，x_base,y_base表示主基站的横、纵坐标值；

步骤4.2、求解坐标系转换后的定位基站的坐标；

利用式(3)得到UWB定位系统的b坐标系下主基站的坐标base_b(x1,y1)：

式(3)中，表示对变换矩阵取逆；

利用式(4)得到UWB定位系统的b坐标系下第一次基站sub1的坐标sub1_b(x₂,y₂)：

利用式(5)得到UWB定位系统的b坐标系下第二次基站sub2的坐标sub2_b(x₂,y₂)：

式(5)中，表示对变换矩阵取逆；从而得到所述UWB定位系统下的三个定位基站的坐标分别为主基站base_b(0,0)，第一次基站sub1_b(x_sub1-x_base,y_sub1-y_base)，第二次基站sub2_b(x_sub2-x_base,y_sub2-y_base)。