[发明专利]大范围、高精度位姿确定方法及其系统

说明书

提供了大范围、高精度位姿确定方法及其系统。所提供的确定物体的位置的方法，包括：获取m个基站到所述物体的n个标签的K个测量的距离；其中，m为大于0的整数，n为大于0的整数，所述n个标签被设置于所述物体，所述n个标签各自位于相对于所述物体的第一点(Po)的标签位置，其中；在定位时，所述物体相对于初始姿态的姿态变化为欧拉角(α,β,γ)；以及所述m个基站各自位于基站位置；根据所述K个测量的距离得到所述欧拉角(α,β,γ)与所述第一点(Po)的位置。

技术领域

本申请涉及定位技术，具体地，涉及使用减少数量的基站实现定位的方法及其系统。

背景技术

定位技术已经被广泛使用。全球定位系统(GPS，Global Positioning System)是定位技术的典型应用。在室内场景中，超宽带(UWB，UItra Wide Band)技术被使用来定位目标。

传统的UWB定位方法主要有两种。一种是基于角度测量。即在设置一个已知位置坐标的基站(anchor，也称为UWB基站)，在待定位的物体上设置电子标签(tag，也简称标签)。在基站上设置多个不同角度的天线组成天线阵列。根据基站各天线的信号强度和相位等来测定标签的信号到达方向(AOA，Angle of Arrival)和标签距基站的距离，从而计算出标签的坐标。

另一种是三角定位。即设置n个已知坐标的基站，在待定位的物体上设置一个标签。用n个基站对标签进行测距，得到n个距离方程，联立n个方程即可求解出标签的具体坐标。图1展示了三角定位系统的示意图。图1的定位系统中部署了三个基站(S1、S2与S3)，其各自具有已知的位置，分别记为S1(x1，y1，z1)，S2(x2，y2，z2)与S3(x3，y3，z3)。物体A是定位系统中的待定位物体。例如，物体A携带UWB标签，而各基站(S1、S2与S3)接收根据从A接收的信号确定各自到物体A的距离(分别记为d1、d2与d3)。可选地，物体A根据从各基站接收的信号确定其到各基站的距离。

记物体A的位置坐标为(x，y，z)，则有：

(x₁-x)²+(y₁-y)²+(z₁-z)²＝d₁²

(x₂-x)²+(y₂-y)²+(z₂-z)²＝d₂²

(x₃-x)²+(y₃-y)²+(z₃-z)²＝d₃²

通过上面3个方程求解物体A的坐标的3个未知数(x，y，z)，理论上可以求解。因为是2次方程，一般有两组解。可以理解为：三个已知球心和半径的球面，求它们的交点。其中两个球面的交点是一个圆。这个圆和三个已知球面中的另一个球面的交点是两个点。

但测距有误差，实际测得的距离可能是d′₁、d′₂与d′₃，代入到上面的方程组右侧(替换d1，d2，d3)，上面的方程可能就没有解了(例如3个球不相交)。这时就需要求满意解(x，y，z)，使得计算值与实测值的差异最小，即，求使下面的式子最小的(x，y，z)：

(d₁-d′₁)²+(d₂-d′₂)²+(d₃-d′₃)²