一种基于角度测量的定位方法与流程

1.本发明总的来说涉及定位技术领域，更具体而言涉及一种基于角度测量的定位方法。


背景技术：

2.随着技术发展，人员或机器(机器人，行驶机械等)对自身定位的需求越来越高。在户外，有美国gps、中国北斗等组成的导航信号网络，人员或机器可持有导航信号接收器，实现自身定位。然而在室内等导航信号中断的环境下，如大型楼宇、地铁站等场所，人员或机器无法通过导航信号进行自身定位。在这些人员或机器活动频繁的场所中，基于安全和寻址，人员或机器具有特别高的定位需求。因此室内导航势必会成为研究热点。
3.目前室内导航大多是基于无线信号执行的，如wifi、5g信号等，但这种方法易因无线信号的多径问题导致定位失败。另外无线信号的功耗问题也困扰着研发定位系统的科研和工作人员。
4.为打破传统方法的局限性，有效实现在室内等导航信号中断的环境下，完成人员或机器的自身定位，需要一种新的设计思路和解决方法。


技术实现要素：

5.本发明的任务是，提供一种基于角度测量的定位方法，通过该方法，可以在室内等导航无线信号中断的环境下，通过部署一套视觉信标，或使用楼宇内特定位置作为视觉信标，测量信标矢量的夹角，并利用最小二乘算法完成人员或机器的定位，由此在缺少导航无线信号的情况下仍然实现精确的导航，提高导航的鲁棒性。
6.根据本发明，前述任务通过一种基于角度测量的定位方法来解决，该方法包括下列步骤：
7.确定要定位物体的视场内至少3个视觉信标，其中所述视觉信标的位置是已知的；
8.确定要定位物体到所述视觉信标中的每两个视觉信标的矢量夹角，其中矢量夹角不小于3个；以及
9.利用最小二乘法原理根据视觉信标的位置坐标和矢量夹角计算要定位物体的坐标。
10.在本发明的一个优选方案中规定，确定要定位物体到所述视觉信标中的每两个视觉信标的矢量夹角包括：
11.根据相似三角形原理通过相机传感器测量矢量夹角，若一次无法看全3个及以上的视觉信标，可通过转动视场完成测角，在转动过程中，要定位物体的位置不可改变。
12.在本发明的另一优选方案中规定，利用最小二乘法原理根据视觉信标的位置坐标和矢量夹角计算要定位物体的坐标包括：
13.在要定位物体与i和j两个信标组成的三角形中，根据余弦定理，两个信标的位置坐标与矢量夹角可建立方程f1；
14.根据两个信标的位置坐标与矢量夹角建立的方程f1构造函数f2；
15.在要定位物体的坐标的近似值处将函数f2泰勒展开,得到公式f3；
16.保留公式f3的一阶项，令函数f＝0可写作方程f4；
17.对于n个信标，其中n≥3，可通过公式f1
‑
f4建立n个方程；联立n个方程f4，得到hx＝b形式的方程组；以及
18.根据最小二乘算法原理进行迭代，求出要定位物体的坐标。
19.在本发明的又一优选方案中规定，在要定位物体与i和j两个信标组成的三角形中，根据余弦定理，两个信标的位置坐标与矢量夹角可建立方程f1为：
20.|r
me
‑
r
i
|2 |r
me
‑
r
j
|2‑
|r
i
‑
r
j
|2＝2*|r
me
‑
r
i
|*|r
me
‑
r
j
|*cos(θ
i
‑
me
‑
j
)，
21.其中r
i
和r
j
为i信标和j信标的位置坐标，可精确测量，θ
i
‑
me
‑
j
为要定位物体到i信标和j信标的两个矢量的夹角，r
me
为要定位物体的坐标，要定位物体的坐标为待求量。
22.在本发明的另一优选方案中规定，该方法还包括：
23.θ
i
‑
me
‑
j
为要定位物体到i信标和j信标的两个矢量的夹角，所述夹角的角度可通过相机传感器测量，测量误差为角秒量级。
24.在本发明的又一优选方案中规定，根据两个信标的位置坐标与矢量夹角建立的方程f1构造函数f2包括：
25.f
i,j
＝|r
me
‑
r
i
|2 |r
me
‑
r
j
|2‑
|r
i
‑
r
j
|2‑
2*|r
me
‑
r
i
|*|r
me
‑
r
j
|*cos(θ
i
‑
me
‑
j
)，通过测量θ
i
‑
me
‑
j
求得r
me
的过程即求f＝0。
26.在本发明的另一优选方案中规定，在要定位物体的坐标的近似值处将函数f2泰勒展开,得到公式f3包括：
27.在r
me
的近似值处将f泰勒展开，得到公式f3：
28.其中为要定位物体的坐标r
me
的近似值。
29.在本发明的又一优选方案中规定，保留公式f3的一阶项，令f＝0可写作公式f4：
[0030][0031]
在本发明的另一优选方案中规定，对于n个信标，其中n≥3，可通过公式f1
‑
f4建立n个方程包括：
[0032]
对于n个信标(n≥3)，可通过公式f1
‑
f4建立个方程，为了方程不冗余，可将信标编号1
‑
n，按照顺序[1,2]，[2,3]，
…
，[n
‑
1,n]，[n,1]为一组建立n个方程。
[0033]
在本发明的又一优选方案中规定，联立n个方程f4，得到hx＝b形式的方程组包括：
[0034]
联立每一组公式f4，得到hx＝b形式的方程组，其中
[0035][0036][0037][0038]
在本发明的另一优选方案中规定，其中根据最小二乘算法原理进行迭代，求出要定位物体的坐标：
[0039]
根据最小二乘算法原理，第k步迭代步骤：
[0040][0041]
更新第k 1步估计值为：
[0042]
以及
[0043]
根据边界条件选迭代初值，设置收敛标志，多轮迭代后，即可求出要定位物体的坐标r
me
。
[0044]
本发明至少具有下列有益效果：(1)本发明提出了一种基于角度测量的定位方法，可以实现在室内等导航信号中断的环境下，通过部署一套视觉信标，或采用楼宇内特定位置作为视觉信标，测量信标矢量的夹角即可完成人员或机器的定位；(2)本发明的方法成本低，算法实现计算量小，定位精度高。
附图说明
[0045]
下面结合附图参考具体实施例来进一步阐述本发明。
[0046]
图1示出了三个视觉信标情况下的矢量示意图；以及
[0047]
图2示出了三个视觉信标情况下的迭代轨迹示意图。
具体实施方式
[0048]
应当指出，各附图中的各组件可能为了图解说明而被夸大地示出，而不一定是比例正确的。在各附图中，给相同或功能相同的组件配备了相同的附图标记。
[0049]
在本发明中，各实施例仅仅旨在说明本发明的方案，而不应被理解为限制性的。
[0050]
在本发明中，除非特别指出，量词“一个”、“一”并未排除多个元素的场景。
[0051]
在此还应当指出，在本发明的实施例中，为清楚、简单起见，可能示出了仅仅一部分部件或组件，但是本领域的普通技术人员能够理解，在本发明的教导下，可根据具体场景需要添加所需的部件或组件。
[0052]
在此还应当指出，在本发明的范围内，“相同”、“相等”、“等于”等措辞并不意味着二者数值绝对相等，而是允许一定的合理误差，也就是说，所述措辞也涵盖了“基本上相同”、“基本上相等”、“基本上等于”。
[0053]
另外，本发明的各方法的步骤的编号并未限定所述方法步骤的执行顺序。除非特别指出，所述方法步骤可以以不同顺序执行。
[0054]
首先阐述本发明所基于的原理。
[0055]
为了解决在室内等导航无线信号中断的环境下，人员或机器无法精确定位的问题，本发明提供了一种基于角度测量的定位方法。该方法通过部署至少三个视觉信标，或使用楼宇内特定位置作为视觉信标，测量信标矢量的夹角，利用最小二乘法原理根据视觉信标的位置坐标和矢量夹角计算出要定位物体的坐标。
[0056]
三点定位法需测量要定位物体到三个信标的距离，采用无线电发射和接收，还可能需要知道所测物体的姿态和角度，测量成本高，操作复杂。相比之下，基于角度测量的定位方法需要测量角度，角度测量的操作简单。
[0057]
图1示出了三个视觉信标情况下的矢量示意图。
[0058]
以三个视觉信标为例，说明本发明的算法步骤。假设视场中存在三个视觉信标，将视觉信标编号为1,2,3。三个视觉信标对应的坐标为r1，r2，r3，视觉信标的坐标可以在事前精确测量。要定位物体的位置称为自身固定点，自身固定点的坐标r
me
为待求量。测得视场中两两矢量夹角为θ1‑
me
‑2，θ2‑
me
‑3，θ3‑
me
‑1，如图1所示。
[0059]
在步骤1，确定编号为1，2，3的视觉信标的坐标：
[0060]
r1＝[30 10 10]；
[0061]
r2＝[200 30 20]；
[0062]
r3＝[0 400 10]；
[0063]
单位为米。
[0064]
在步骤2，测量角度为真值角度加测量误差10角秒：
[0065]
θ1‑
me
‑2＝78.9298
°
0.0028
°
；
[0066]
θ2‑
me
‑3＝90.4424
°‑
0.0028
°
；
[0067]
θ3‑
me
‑1＝93.1119
°
0.0028
°
。
[0068]
在步骤3，根据边界条件，选取迭代初值：
[0069]
迭代初值
[0070]
在步骤4，设置迭代收敛条件：
[0071]
norm(x
i
)<1e
‑
3。
[0072]
在步骤5，经过5轮迭代的结果为：
[0073][0074]
计算结果与真值的误差在0.01米量级。
[0075]
图2示出了三个信标情况下的迭代轨迹示意图。
[0076]
根据边界条件选取一个迭代初值，通过多轮迭代，计算结果不断趋近于真实值。本发明可通过减小迭代收敛阈值提升定位精度，但会增加计算量，使用者可平衡两者权重。随着测量技术的提升，本发明的定位精度还会提升。
[0077]
本发明至少具有下列有益效果：(1)本发明提出了一种基于角度测量的定位方法，可以实现在室内等导航信号中断的环境下，通过部署一套视觉信标，或采用楼宇内特定位置作为视觉信标，测量信标矢量的夹角即可完成人员或机器的定位。(2)本发明的方法成本低，算法实现计算量小，定位精度高。
[0078]
虽然本发明的一些实施方式已经在本技术文件中予以了描述，但是本领域技术人员能够理解，这些实施方式仅仅是作为示例示出的。本领域技术人员在本发明的教导下可以想到众多的变型方案、替代方案和改进方案而不超出本发明的范围。所附权利要求书旨在限定本发明的范围，并藉此涵盖这些权利要求本身及其等同变换的范围内的方法和结构。