基于距离检测的物体三维姿态角判定方法、系统及设备与流程

1.本发明涉及距离检测的技术领域，具体涉及一种基于距离检测的物体三维姿态角判定方法、系统及设备。


背景技术：

2.传统的物体姿态角度检测方法主要采用图像处理技术，难以满足对于三维场景的高精度、实时性等要求。而基于距离检测的方法则可通过全站仪、激光雷达、uwb等设备对场景中的物体进行距离测量，获取物体在三维空间中的几何信息，具有相对较好的稳定性和抗噪声能力。
3.现代全站仪比较常用的leica为例，其测量精度可以达到0.7"(秒)，配合反光棱镜使用时误差距离能够达到2mm+2ppm，而测量范围可达到数百米。并且很多全站仪可以实现快速测量模式，极大地缩短了角度和距离测量时间，有些还配备了内置的操作系统，可以自动将测量数据转换为电子格式。
4.但是，目前提取目标物体的距离信息特征，并计算出其在三维坐标系下姿态角度的处理过程相对复杂，导致物体姿态检测时的姿态实时性和准确性不足。


技术实现要素：

5.针对现有技术中所存在的不足，本发明提供了基于距离检测的物体三维姿态角判定方法、系统及设备，以解决现有技术中提取目标物体的距离信息特征，并计算出其在三维坐标系下姿态角度的处理过程相对复杂，导致物体姿态检测时的姿态实时性和准确性不足的技术问题。
6.本发明提供了一种基于距离检测的物体三维姿态角判定方法，包括：
7.s1、在待测目标物体上布置三个目标点，通过测距仪器，获取观测点o点和三个目标点x1,x2,x3之间的距离，并确保观测点o点到三个目标点x1,x2,x3的距离d1,d2,d3相同；
8.s2、创建以观测点o点为基础的三维直角坐标系，p点是由三个目标点构成的等边三角形的重心，以op方向为x轴，垂直于op，平行于x2,x3直线方向为y轴，垂直于op，平行于px1直线方向为z轴建立坐标系，并分别设俯仰角θ绕y轴旋转，航向角ψ绕z轴旋转，横滚角γ绕x轴旋转；
9.s3、基于所述三维直角坐标系，解算出所述待测目标物体在其下的初始姿态角，并实时观测所述待测目标物体从初始时刻和后一时刻通过距离变化而产生姿态角的偏移，对其解算，通过变化的阈值范围来确定所述待测目标物体下一时刻的姿态角信息；
10.s4、将所述姿态角信息传输反馈至终端设备，结合设于所述待测目标物体上的传感器的检测数据，生成姿态判定结果。
11.可选地，所述基于所述三维直角坐标系，解算出所述待测目标物体在其下的初始姿态角，包括：
12.设所述待测目标物体姿态变化时，p在空间中的位置不变，所在平面垂直于等边
三角形所在平面，则：
[0013][0014]
同时，通过所述距离的测距信息来不断修正观测点o点的位置，修正方法为：
[0015][0016]
则得到所述待测目标物体初始时刻的位置信息为：
[0017][0018]
可选地，所述并实时观测所述待测目标物体从初始时刻和后一时刻通过距离变化而产生姿态角的偏移，并对其解算，包括：
[0019]
解算所述俯仰角θ，设初始所述俯仰角θ为90
°
，俯仰角的取值范围是0
°
到180
°
，所述待测目标物体只有俯仰角变化时，所述俯仰角的变化转化为op和px1这两条直线的夹角变化，所述俯仰角θ的解算过程为：
[0020][0021]
其中，di'为某一时刻的di的测量值，进一步可解俯仰角θ：
[0022][0023]
可选地，所述并实时观测所述待测目标物体从初始时刻和后一时刻通过距离变化而产生姿态角的偏移，并对其解算，还包括：
[0024]
解算所述航向角ψ，设初始所述航向角ψ为90
°
，航向角的取值范围是0
°
到180
°
，构造一个过的平面oqp，且所述平面oqp法向量与平行，所述待测目标物体只有俯仰角和航向角变化时，或只有航向角变化时，航向角的变化转换为三维直角坐标系中op和pq两条直线的夹角变化，其中航向角ψ＝∠opq，则可得：
[0025]
[0026][0027]
并可解算得到：
[0028][0029]
同理可得：
[0030][0031]
则可以得到更加精确的航向角信息：
[0032]
ψ＝(ψ0+ψ1)/2。
[0033]
可选地，所述并实时观测所述待测目标物体从初始时刻和后一时刻通过距离变化而产生姿态角的偏移，并对其解算，还包括：
[0034]
解算所述横滚角γ，设初始所述横滚角γ为0
°
，横滚角的取值范围是0
°
到360
°
，引入另一个观测点o1，所在的平面也垂直于所述等边三角形所在平面，且初始时刻在三角形px1x'1中，所述横滚角γ转换成γ＝∠x1px'1，则所述横滚角γ的解算过程为：
[0035][0036][0037]
并可解算得到：
[0038][0039]
可选地，所述横滚角γ的解算过程之前，还包括：
[0040]
当d'2＞d'3，横滚角为γ，当d'2＜d'3时，横滚角为360
°‑
γ。
[0041]
本发明还提供了一种基于距离检测的物体三维姿态角判定系统，包括：
[0042]
准备模块，用于在待测目标物体上布置三个目标点，通过测距仪器，获取观测点o点和三个目标点x1,x2,x3之间的距离，并确保观测点o点到三个目标点x1,x2,x3的距离d1,d2,d3相同；
[0043]
设置模块，用于创建以观测点o点为基础的三维直角坐标系，p点是由三个目标点构成的等边三角形的重心，以op方向为x轴，垂直于op，平行于x2,x3直线方向为y轴，垂直于op，平行于px1直线方向为z轴建立坐标系，并分别设俯仰角θ绕y轴旋转，航向角ψ绕z轴旋
转)，横滚角γ绕x轴旋转；
[0044]
解算模块，用于通过实时观测所述待测目标物体从初始时刻和后一时刻通过距离变化而产生姿态角的偏移，并对其解算，通过变化的阈值范围来确定所述待测目标物体下一时刻的姿态角信息；
[0045]
反馈模块，将所述姿态角信息传输反馈至终端设备，结合设于所述待测目标物体上的传感器的检测数据，生成姿态判定结果。
[0046]
本发明还提供了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现前述方法的步骤。
[0047]
相比于现有技术，本发明具有如下有益效果：
[0048]
本发明可在高精度的测距装置的数据反馈下，基于自身创建的三维直角坐标系，解算出物体在三维坐标系下的初始姿态角，并以此为初始样本作为后续姿态角偏移计算的参考数据，以此确定待测目标物体在下一时刻的姿态角信息，并反馈给后台终端，本发明对进行物体的三维姿态角判定时，所需设备简单，易于操作，处理的数据量小，判定识别耗时短，实时性较强，对物体的三维姿态角的测量精度的提高具有较强的实用性。
附图说明
[0049]
此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本发明的实施例，并与说明书一起用于解释本发明的原理。
[0050]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，对于本领域普通技术人员而言，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
[0051]
图1为本发明中的方法流程示意图；
[0052]
图2为本发明中被测目标物体的三维示意图；
[0053]
图3为本发明中被测目标物体的二维示意图；
[0054]
图4为本发明中航向角解算示意图；
[0055]
图5为本发明中俯仰角解算示意图；
[0056]
图6为本发明中俯仰角解算二维示意图a；
[0057]
图7为本发明中俯仰角解算二维示意图b。
具体实施方式
[0058]
为使本技术实施例的目的、技术方案和优点更加清楚，下面将结合本技术实施例中的附图，对本技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本技术的一部分实施例，而不是全部的实施例。基于本技术中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本技术保护的范围。本发明实例中相同标号的功能单元具有相同和相似的结构和功能。
[0059]
参见图1，本发明提供了一种基于距离检测的物体三维姿态角判定方法，包括：
[0060]
s1、在待测目标物体上布置三个目标点，通过测距仪器，获取观测点o点和三个目标点x1,x2,x3之间的距离，并确保观测点o点到三个目标点x1,x2,x3的距离d1,d2,d3相同；
[0061]
s2、创建以观测点o点为基础的三维直角坐标系，p点是由三个目标点构成的等边
三角形的重心，以op方向为x轴，垂直于op，平行于x2,x3直线方向为y轴，垂直于op，平行于px1直线方向为z轴建立坐标系，并分别设俯仰角θ绕y轴旋转，航向角ψ绕z轴旋转，横滚角γ绕x轴旋转；
[0062]
s3、基于所述三维直角坐标系，解算出所述待测目标物体在其下的初始姿态角，并实时观测所述待测目标物体从初始时刻和后一时刻通过距离变化而产生姿态角的偏移，对其解算，通过变化的阈值范围来确定所述待测目标物体下一时刻的姿态角信息；
[0063]
s4、将所述姿态角信息传输反馈至终端设备，结合设于所述待测目标物体上的传感器的检测数据，生成姿态判定结果。
[0064]
本实施例中，本发明所采用的测距仪器，优选精准度高的全站仪、uwb等，并且所述测距仪器采用的设备优选以有线传输为主，以此确保数据的实时性和减少数据的丢包率。同时，采集的数据在处理时，可以引入滤波算法来提高数据的精度和有效性。在姿态角信息反馈时，可以结合所述待测目标物体上的传感器的检测数据，来生成最后的姿态判定结果，所述传感器可以采用惯性测量设备等。
[0065]
s1、在待测目标物体上布置三个目标点，通过测距仪器，获取观测点o点和三个目标点x1,x2,x3之间的距离，并确保观测点o点到三个目标点x1,x2,x3的距离d1,d2,d3相同。
[0066]
参见图2和图3，s2、创建以观测点o点为基础的三维直角坐标系，p点是由三个目标点构成的等边三角形的重心，以op方向为x轴，垂直于op，平行于x2,x3直线方向为y轴，垂直于op，平行于px1直线方向为z轴建立坐标系，并分别设俯仰角θ绕y轴旋转，航向角ψ绕z轴旋转，横滚角γ绕x轴旋转；
[0067]
s3、基于所述三维直角坐标系，解算出所述待测目标物体在其下的初始姿态角，并实时观测所述待测目标物体从初始时刻和后一时刻通过距离变化而产生姿态角的偏移，对其解算，通过变化的阈值范围来确定所述待测目标物体下一时刻的姿态角信息；
[0068]
根据前述布置的观测点和目标点的信息后，首先需要解算述待测目标物体在其下的初始姿态角。参见图2，p为等边三角形x1x2x3的重心，设所述待测目标物体姿态变化时，p在空间中的位置不变，所在平面垂直于等边三角形所在平面，则：
[0069][0070]
由于x1,x2,x3,p都是提前布置好的点，可以通过所述距离的测距信息来不断修正观测点o点的位置，修正方法为：
[0071][0072]
则得到所述待测目标物体初始时刻的位置信息为：
[0073][0074]
由于设俯仰角θ(绕y旋转)，航向角ψ(绕z旋转)，横滚角γ(绕x旋转)。
[0075]
为在后续的姿态结算中提供便利，因此可设置：
[0076]
初始俯仰角θ为90
°
，俯仰角的取值范围是0
°
到180
°
。
[0077]
初始航向角ψ为90
°
，航向角的取值范围是0
°
到180
°
[0078]
初始横滚角γ为0
°
，横滚角的取值及范围是0
°
到360
°
。
[0079]
并依次解算俯仰角θ、航向角ψ及横滚角γ；
[0080]
参见图2，所述待测目标物体只有俯仰角变化时，由几何关系可知，所述俯仰角的变化转化为op和px1这两条直线的夹角变化，所述俯仰角θ的解算过程为：
[0081][0082]
其中，di'为某一时刻的di的测量值，进一步可解俯仰角θ：
[0083][0084]
由于条件的特殊性，姿态变化时重心在空间中的位置不会改变，如果只有俯仰角的变化，则基本不会影响到航向角的测量。
[0085]
参见图4，其次，解算航向角，构造一个过的平面oqp，且所述平面oqp法向量与平行，所述待测目标物体只有俯仰角和航向角变化时，或只有航向角变化时，航向角的变化转换为三维直角坐标系中op和pq两条直线的夹角变化，其中航向角ψ＝∠opq，则可得：
[0086][0087][0088]
并可解算得到：
[0089][0090]
同理可得：
[0091][0092]
则可以得到更加精确的航向角信息：
[0093]
ψ＝(ψ0+ψ1)/2。
[0094]
参见图5，最后，解算所述横滚角γ，设初始所述横滚角γ为0
°
，横滚角的取值范围是0
°
到360
°
，引入另一个观测点o1，所在的平面也垂直于所述等边三角形所在平面，且初始时刻在三角形px1x'1中，所述横滚角γ转换成γ＝∠x1px'1，则所述横滚角γ的解算过程为：
[0095][0096][0097]
并可解算得到：
[0098][0099]
参见图6，当d'2＞d'3，横滚角为γ；
[0100]
参见图7，当d2'＜d'3时，横滚角为360
°‑
γ。
[0101]
通过解算出的航向角、俯仰角、横滚角，通过变化的阈值范围来确定所述待测目标物体下一时刻的姿态角信息，并且姿态角信息结合所述待测目标物体上的传感器的检测数据均发送至终端设备，生成姿态判定结果。
[0102]
本发明可在高精度的测距装置的数据反馈下，基于自身创建的三维直角坐标系，解算出物体在三维坐标系下的初始姿态角，并以此为初始样本作为后续姿态角偏移计算的参考数据，以此确定待测目标物体在下一时刻的姿态角信息，并反馈给后台终端，本发明对进行物体的三维姿态角判定时，所需设备简单，易于操作，处理的数据量小，判定识别耗时短，实时性较强，对物体的三维姿态角的测量精度的提高具有较强的实用性。
[0103]
本发明还提供了一种基于距离检测的物体三维姿态角判定系统，包括：
[0104]
准备模块，用于在待测目标物体上布置三个目标点，通过测距仪器，获取观测点o点和三个目标点x1,x2,x3之间的距离，并确保观测点o点到三个目标点x1,x2,x3的距离d1,d2,d3相同；
[0105]
设置模块，用于创建以观测点o点为基础的三维直角坐标系，p点是由三个目标点构成的等边三角形的重心，以op方向为x轴，垂直于op，平行于x2,x3直线方向为y轴，垂直于op，平行于px1直线方向为z轴建立坐标系，并分别设俯仰角θ绕y轴旋转，航向角ψ绕z轴旋转)，横滚角γ绕x轴旋转；
[0106]
解算模块，用于通过实时观测所述待测目标物体从初始时刻和后一时刻通过距离
变化而产生姿态角的偏移，并对其解算，通过变化的阈值范围来确定所述待测目标物体下一时刻的姿态角信息；
[0107]
反馈模块，将所述姿态角信息传输反馈至终端设备，结合设于所述待测目标物体上的传感器的检测数据，生成姿态判定结果。
[0108]
本发明还提供了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现前述方法的步骤。
[0109]
需要说明的是，在本文中，诸如“第一”和“第二”等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个
……”
限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
[0110]
以上所述仅是本发明的具体实施方式，使本领域技术人员能够理解或实现本发明。对这些实施例的多种修改对本领域的技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所申请的原理和新颖特点相一致的最宽的范围。

技术特征：
1.一种基于距离检测的物体三维姿态角判定方法，其特征在于，包括：s1、在待测目标物体上布置三个目标点，通过测距仪器，获取观测点o点和三个目标点x1,x2,x3之间的距离，并确保观测点o点到三个目标点x1,x2,x3的距离d1,d2,d3相同；s2、创建以观测点o点为基础的三维直角坐标系，p点是由三个目标点构成的等边三角形的重心，以op方向为x轴，垂直于op，平行于x2,x3直线方向为y轴，垂直于op，平行于px1直线方向为z轴建立坐标系，并分别设俯仰角θ绕y轴旋转，航向角ψ绕z轴旋转，横滚角γ绕x轴旋转；s3、基于所述三维直角坐标系，解算出所述待测目标物体在其下的初始姿态角，并实时观测所述待测目标物体从初始时刻和后一时刻通过距离变化而产生姿态角的偏移，对其解算，通过变化的阈值范围来确定所述待测目标物体下一时刻的姿态角信息；s4、将所述姿态角信息传输反馈至终端设备，结合设于所述待测目标物体上的传感器的检测数据，生成姿态判定结果。2.如权利要求1所述的基于距离检测的物体三维姿态角判定方法，其特征在于，所述基于所述三维直角坐标系，解算出所述待测目标物体在其下的初始姿态角，包括：设所述待测目标物体姿态变化时，p在空间中的位置不变，所在平面垂直于等边三角形所在平面，则：同时，通过所述距离的测距信息来不断修正观测点o点的位置，修正方法为：则得到所述待测目标物体初始时刻的位置信息为：3.如权利要求2所述的基于距离检测的物体三维姿态角判定方法，其特征在于，所述并实时观测所述待测目标物体从初始时刻和后一时刻通过距离变化而产生姿态角的偏移，并对其解算，包括：解算所述俯仰角θ，设初始所述俯仰角θ为90
°
，俯仰角的取值范围是0
°
到180
°
，所述待测目标物体只有俯仰角变化时，所述俯仰角的变化转化为op和px1这两条直线的夹角变化，所述俯仰角θ的解算过程为：
其中，d'
i
为某一时刻的d
i
的测量值，进一步可解俯仰角θ：4.如权利要求3所述的基于距离检测的物体三维姿态角判定方法，其特征在于，所述并实时观测所述待测目标物体从初始时刻和后一时刻通过距离变化而产生姿态角的偏移，并对其解算，还包括：解算所述航向角ψ，设初始所述航向角ψ为90
°
，航向角的取值范围是0
°
到180
°
，构造一个过的平面oqp，且所述平面oqp法向量与平行，所述待测目标物体只有俯仰角和航向角变化时，或只有航向角变化时，航向角的变化转换为三维直角坐标系中op和pq两条直线的夹角变化，其中航向角ψ＝∠opq，则可得：的夹角变化，其中航向角ψ＝∠opq，则可得：并可解算得到：同理可得：则可以得到更加精确的航向角信息：ψ＝(ψ0+ψ1)/2。5.如权利要求4所述的基于距离检测的物体三维姿态角判定方法，其特征在于，所述并实时观测所述待测目标物体从初始时刻和后一时刻通过距离变化而产生姿态角的偏移，并对其解算，还包括：解算所述横滚角γ，设初始所述横滚角γ为0
°
，横滚角的取值范围是0
°
到360
°
，引入另一个观测点o1，所在的平面也垂直于所述等边三角形所在平面，且初始时刻在三角形px1x'1中，所述横滚角γ转换成γ＝∠x1px'1，则所述横滚角γ的解算过程为：
并可解算得到：6.如权利要求5所述的基于距离检测的物体三维姿态角判定方法，其特征在于，所述横滚角γ的解算过程之前，还包括：当d'2＞d'3，横滚角为γ，当d'2＜d'3时，横滚角为360
°‑
γ。7.一种基于距离检测的物体三维姿态角判定系统，其特征在于，包括：准备模块，用于在待测目标物体上布置三个目标点，通过测距仪器，获取观测点o点和三个目标点x1,x2,x3之间的距离，并确保观测点o点到三个目标点x1,x2,x3的距离d1,d2,d3相同；设置模块，用于创建以观测点o点为基础的三维直角坐标系，p点是由三个目标点构成的等边三角形的重心，以op方向为x轴，垂直于op，平行于x2,x3直线方向为y轴，垂直于op，平行于px1直线方向为z轴建立坐标系，并分别设俯仰角θ绕y轴旋转，航向角ψ绕z轴旋转)，横滚角γ绕x轴旋转；解算模块，用于通过实时观测所述待测目标物体从初始时刻和后一时刻通过距离变化而产生姿态角的偏移，并对其解算，通过变化的阈值范围来确定所述待测目标物体下一时刻的姿态角信息；反馈模块，将所述姿态角信息传输反馈至终端设备，结合设于所述待测目标物体上的传感器的检测数据，生成姿态判定结果。8.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1-6任一项所述方法的步骤。

技术总结
本发明提供了一种基于距离检测的物体三维姿态角判定方法，本发明可在高精度的测距装置的数据反馈下，基于自身创建的三维直角坐标系，解算出物体在三维坐标系下的初始姿态角，并以此为初始样本作为后续姿态角偏移计算的参考数据，以此确定待测目标物体在下一时刻的姿态角信息，并反馈给后台终端，本发明对进行物体的三维姿态角判定时，所需设备简单，易于操作，处理的数据量小，判定识别耗时短，实时性较强，对物体的三维姿态角的测量精度的提高具有较强的实用性。有较强的实用性。有较强的实用性。


技术研发人员：舒宇 王三成 习震武 王麒超
受保护的技术使用者：中国通信建设第二工程局有限公司
技术研发日：2023.06.30
技术公布日：2023/10/5