一种空间位置测量装置与方法与流程

1.本发明属于空间位置测量技术领域，具体涉及一种空间位置测量装置与方法。


背景技术：

2.通常，我们在测量空间两个待测点的位置关系时，会使用尺子或激光测距仪进行测量，得到空间两个待测点之间的距离，并结合坐标系得到两个待测点的空间位置。但是当空间两个待测点之间存在障碍物时，使用上述的方法则无法测量或者不方便测量。另外，在确定空间两个待测点的位置的时候，由于测量时使尺子方向与坐标系的轴方向垂直也比较困难，导致测量误差较大。


技术实现要素：

3.为克服上述现有技术的不足，本发明提出一种空间位置测量装置与方法。
4.本发明提供的技术方案如下：
5.本发明提出了一种空间位置测量装置，包括至少两个连杆，相邻的所述连杆之间首尾转动连接；
6.每一个所述连杆上均设置有一个陀螺仪，所述陀螺仪位于测量平面内，所述测量平面由全部所述连杆以及两个待测空间点所在直线围成；
7.每一个所述陀螺仪分别与控制板通信连接，每一个所述陀螺仪分别与控制板电连接，所述控制板用于提供电源和数据处理。
8.作为优选的，所述转动连接为铰接，相邻的所述连杆之间具有相匹配的铰接点。
9.作为优选的，所述陀螺仪与所述控制板之间通过导线连接。
10.基于同一发明构思，本发明还提出了一种空间位置测量方法，包括：
11.将装置放置在待测空间点所在的原始坐标系下，使得装置中的陀螺仪中三轴坐标系与所述原始坐标系重合，得到每一个陀螺仪的初始姿态角并计算对应的旋转矩阵；
12.将两个待测空间点与装置中首尾连杆的自由端相接触，分别获取每一个连杆上陀螺仪的最终姿态角并计算对应的旋转矩阵；
13.根据所述初始姿态角对应的旋转矩阵、所述最终姿态角对应的旋转矩阵以及对应的连杆长度，计算两个待测空间点之间的空间位置关系矩阵；
14.根据所述空间位置关系矩阵计算两个待测空间点之间的距离；
15.其中，所述装置为上述的一种空间位置测量装置。
16.作为优选的，所述计算两个待测空间点之间的空间位置关系矩阵，包括：
17.根据每一个陀螺仪的初始姿态角对应的旋转矩阵和最终姿态角对应的旋转矩阵计算每一个陀螺仪对应的转换矩阵；
18.根据每一个陀螺仪对应的转换矩阵和对应的连杆长度计算所述空间位置关系矩阵。
19.作为优选的，所述根据每一个陀螺仪对应的转换矩阵和对应的连杆长度计算所述
空间位置关系矩阵，通过以下计算式：
20.(x,y,z)
t
＝r1(l1,0,0)
t
+r2(l2,0,0)
t
+
…
+rn(ln,0,0)
t
+
…
+rn(ln,0,0)
t
21.其中，(x,y,z)
t
为所述空间位置关系矩阵，rn为第n个陀螺仪对应的转换矩阵，ln第n个陀螺仪所在连杆的长度，n为所述连杆的总数。
22.作为优选的，所述根据每一个陀螺仪的初始姿态角对应的旋转矩阵和最终姿态角对应的旋转矩阵计算每一个陀螺仪对应的转换矩阵，通过以下计算式：
23.rn＝r
n(2)-1rn(1)
24.其中，r
n(1)
为第n个陀螺仪的初始姿态角对应的旋转矩阵，r
n(2)
为第n个陀螺仪的最终姿态角对应的旋转矩阵。
25.作为优选的，所述根据所述空间位置关系矩阵计算两个待测空间点之间的距离，通过以下计算式：
[0026][0027]
其中，d为两个待测空间点之间的距离，x、y、z分别为所述空间位置关系矩阵(x,y,z)
t
中的分量值。
[0028]
基于同一发明构思，本发明又提出了一种计算机设备，包括：一个或多个处理器；存储器，用于存储一个或多个程序；
[0029]
当所述一个或多个程序被所述一个或多个处理器执行时，实现上述的一种空间位置测量方法。
[0030]
基于同一发明构思，本发明再提出了一种计算机可读存储介质，其上存有计算机程序，所述计算机程序被执行时，实现上述的一种空间位置测量方法。
[0031]
与最接近的现有技术相比，本发明具有的有益效果如下：
[0032]
本发明提供了一种空间位置测量装置与方法，该装置包括至少两个连杆，相邻的所述连杆之间首尾转动连接；每一个所述连杆上均设置有一个陀螺仪，所述陀螺仪位于测量平面内，所述测量平面由全部所述连杆以及两个待测空间点所在直线围成；每一个所述陀螺仪分别与控制板通信连接，每一个所述陀螺仪分别与控制板电连接，所述控制板用于提供电源和数据处理。本发明通过将多连杆的两端与待测量的两个点接触，读取每一个连杆上陀螺仪的姿态角数据，从而可以计算出空间两个点的距离及位置关系，进而实现了在两个点之间存在障碍物的时候，也可以完成两个点之间位置关系和距离的测量。
附图说明
[0033]
图1为本发明提出的一种空间位置测量装置的结构示意图；
[0034]
图2为本发明的装置中陀螺仪的安装示意图；
[0035]
图3为本发明提出的一种空间位置测量方法的流程示意图；
[0036]
图4为本发明实施例2中的待测空间点的位置示意图。
[0037]
附图标记说明：
[0038]
11、第一连杆；12、第二连杆；21、第一陀螺仪；22、第二陀螺仪；3、控制板；4、铰接点；5、导线。
具体实施方式
[0039]
下面结合附图对本发明的具体实施方式做进一步的详细说明。
[0040]
实施例1：
[0041]
本发明提供了一种空间位置测量装置，如图1所示，该空间位置测量装置包括至少两个连杆，相邻的所述连杆之间首尾转动连接；图1中以两个连杆为例，分别是第一连杆11和第二连杆12，其之间通过铰接点4铰接，使得相邻的两个第一连杆11和第二连杆12能够围绕铰接点4相互转动。图中，a、b为两个待测空间点。
[0042]
每一个所述连杆上均设置有一个陀螺仪，所述陀螺仪位于测量平面内，所述测量平面由全部所述连杆以及两个待测空间点所在直线围成；如图2所示，图2是陀螺仪的安装示意图，在各个连杆平铺为一条直线的时候，陀螺仪如图所示，这样在两个点之间存在障碍物的时候，多个连杆之间形成弯折的形式，此时，每一个连杆以及两个待测空间点所在直线就围成了一个平面，即为测量平面，各个连杆上的陀螺仪就位于测量平面内。在图1中，第一连杆11上对应安装的第一陀螺仪21，第二连杆12上对应安装的第二陀螺仪22，第一陀螺仪21和第二陀螺仪22均属于陀螺仪。
[0043]
如图1所示，每一个所述陀螺仪分别与控制板3通信连接，每一个所述陀螺仪分别与控制板3电连接，所述控制板用于提供电源和数据处理。在使用的时候，将多连杆的两端与待测量的两个点接触，之后，通过控制板3读取每一个连杆上陀螺仪的姿态角数据，在通过控制板3计算出空间两个点的距离及位置关系。在此过程中，供电也是由控制板3完成，进而使得在两个点之间存在障碍物的时候，也可以完成两个点之间位置关系和距离的测量。
[0044]
其中，在本发明中，所述转动连接为铰接，相邻的所述连杆之间具有相匹配的铰接点4。所述陀螺仪与所述控制板之间通过导线5连接。
[0045]
实施例2：
[0046]
基于同一发明构思，本发明提供了一种空间位置测量方法，如图3所示，包括以下步骤：
[0047]
步骤1：将装置放置在待测空间点所在的原始坐标系下，使得装置中的陀螺仪中三轴坐标系与所述原始坐标系重合，得到每一个陀螺仪的初始姿态角并计算对应的旋转矩阵；
[0048]
步骤2：将两个待测空间点与装置中首尾连杆的自由端相接触，分别获取每一个连杆上陀螺仪的最终姿态角并计算对应的旋转矩阵；
[0049]
步骤3：根据所述初始姿态角对应的旋转矩阵、所述最终姿态角对应的旋转矩阵以及对应的连杆长度，计算两个待测空间点之间的空间位置关系矩阵；
[0050]
步骤4：根据所述空间位置关系矩阵计算两个待测空间点之间的距离；
[0051]
其中，所述装置为上述的一种空间位置测量装置。
[0052]
在步骤1中，是得到在初始状态的每一个陀螺仪的初始姿态角及对应的旋转矩阵，在步骤2中，是得到在测量状态下的每一个陀螺仪的最终姿态角及对应的旋转矩阵。
[0053]
步骤3中，即是将上述已经得到的数值进行计算，如下，所述计算两个待测空间点之间的空间位置关系矩阵，包括：
[0054]
◎
根据每一个陀螺仪的初始姿态角对应的旋转矩阵和最终姿态角对应的旋转矩阵计算每一个陀螺仪对应的转换矩阵；
[0055]
◎
根据每一个陀螺仪对应的转换矩阵和对应的连杆长度计算所述空间位置关系矩阵。
[0056]
基于上述的计算过程，所述根据每一个陀螺仪对应的转换矩阵和对应的连杆长度计算所述空间位置关系矩阵，通过以下计算式：
[0057]
(x,y,z)
t
＝r1(l1,0,0)
t
+r2(l2,0,0)
t
+
…
+rn(ln,0,0)
t
+
…
+rn(ln,0,0)
t
[0058]
其中，(x,y,z)
t
为所述空间位置关系矩阵，rn为第n个陀螺仪对应的转换矩阵，ln第n个陀螺仪所在连杆的长度，n为所述连杆的总数。
[0059]
基于此，所述根据每一个陀螺仪的初始姿态角对应的旋转矩阵和最终姿态角对应的旋转矩阵计算每一个陀螺仪对应的转换矩阵，通过以下计算式：
[0060]rn
＝r
n(2)-1rn(1)
[0061]
其中，r
n(1)
为第n个陀螺仪的初始姿态角对应的旋转矩阵，r
n(2)
为第n个陀螺仪的最终姿态角对应的旋转矩阵。
[0062]
基于此，所述根据所述空间位置关系矩阵计算两个待测空间点之间的距离，通过以下计算式：
[0063][0064]
其中，d为两个待测空间点之间的距离，x、y、z分别为所述空间位置关系矩阵(x,y,z)
t
中的分量值。
[0065]
基于上述的理论，我们以图1所示的装置为例，计算两个待测空间点a和b之间的位置关系和距离。其中，我们假设陀螺仪采用的三轴坐标系(世界坐标系)为东北天坐标系，陀螺仪输出的坐标系旋转顺序为z-y-x，即先要z轴旋转，再绕y轴旋转，再绕x轴旋转。其他陀螺仪原理相同。其中，两个连杆的长度均为1米。
[0066]
如图4所示，需要测量空间中，o、p两点的距离及在原始坐标系下的相对位置关系时，测量方法如下：
[0067]
1：标定。
[0068]
将测量装置放置在待测空间的原始坐标系(参考坐标系)下，使两个陀螺仪三轴坐标系与待测空间的原始坐标系重合，记录两个陀螺仪输出的姿态角，分别为，第一连杆11为(α
10
,β
10
,γ
10
)，第二连杆12为(α
20
,β
20
,γ
20
)。
[0069]
则第一连杆11从初始状态到世界坐标系的旋转矩阵为：r1＝rz(γ
10
)ry(β
10
)r
x
(α
10
)
[0070]
第二连杆12从初始状态到世界坐标系的旋转矩阵为：r2＝rz(γ
20
)ry(β
20
)r
x
(α
20
)
[0071]
其中旋转矩阵的公式为：
[0072][0073][0074]
[0075]
2：测量。
[0076]
将连杆的a点与被测的起点o接触，两个连杆铰链处可以呈任意角度，使将连杆的b点与被测的终点p接触，控制板3采集记录第一陀螺仪21和第二陀螺仪22的欧拉角数据，分别为(α
11
,β
11
,γ
11
)，(α
21
,β
21
,γ
21
)。
[0077]
同理：
[0078]
则第一连杆11从测量状态到世界坐标系的旋转矩阵为：r3＝rz(γ
11
)ry(β
11
)r
x
(α
11
)
[0079]
则第二连杆12从测量状态到世界坐标系的旋转矩阵为：r4＝rz(γ
21
)ry(β
21
)r
x
(α
21
)
[0080]
3：计算。
[0081]
计算测量状态到参考坐标系的旋转矩阵：
[0082]
第一连杆11从测量状态到初始状态的旋转矩阵为：r5＝r
3-1
r1[0083]
第二连杆12从测量状态到初始状态的旋转矩阵为：r6＝r
4-1
r2[0084]
因连杆长度为1m，即是第一连杆11和第二连杆12的长度均为1米。
[0085]
则，a、b两点的空间位置关系为：
[0086]
(x,y,z)
t
＝r5(1,0,0)
t
+r6(1,0,0)
t
[0087]
则，a、b两点的距离为：
[0088][0089]
基于此，对于多个连杆，假设连杆长度分别是l1，l2，
……
，ln，使用上述方法，先对每一个连杆进行标定，测量时使多连杆的首尾与被测点的两端接触，读取每个连杆的陀螺仪数据后，可以得到每个杆从测量状态到参考坐标系的旋转矩阵r1，r2，
……
，rn，则多连杆两端的空间关系为：
[0090]
(x,y,z)
t
＝r1(l1,0,0)
t
+r2(l2,0,0)
t
+
…
+rn(ln,0,0)
t
[0091]
因此，多连杆两端的距离为：
[0092]
综上，本发明通过将多连杆的两端与待测量的两个点接触，读取每一个连杆上陀螺仪的姿态角数据，从而可以计算出空间两个点的距离及位置关系，进而实现了在两个点之间存在障碍物的时候，也可以完成两个点之间位置关系和距离的测量。
[0093]
实施例3：
[0094]
基于同一种发明构思，本发明还提供了一种计算机设备，该计算机设备包括处理器以及存储器，所述存储器用于存储计算机程序，所述计算机程序包括程序指令，所述处理器用于执行所述计算机存储介质存储的程序指令。处理器可能是中央处理单元(central processing unit，cpu)，还可以是其他通用处理器、数字信号处理器(digital signal processor、dsp)、专用集成电路(application specificintegrated circuit，asic)、现成可编程门阵列(field-programmable gatearray，fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等，其是终端的计算核心以及控制核心，其适于实现一条或一条以上指令，具体适于加载并执行计算机存储介质内一条或一条以上指令从而实现相应方法流程或相应功能，以实现上述实施例中一种空间位置测量方法的步骤。
[0095]
实施例4：
[0096]
基于同一种发明构思，本发明还提供了一种存储介质，具体为计算机可读存储介质(memory)，所述计算机可读存储介质是计算机设备中的记忆设备，用于存放程序和数据。可以理解的是，此处的计算机可读存储介质既可以包括计算机设备中的内置存储介质，当然也可以包括计算机设备所支持的扩展存储介质。计算机可读存储介质提供存储空间，该存储空间存储了终端的操作系统。并且，在该存储空间中还存放了适于被处理器加载并执行的一条或一条以上的指令，这些指令可以是一个或一个以上的计算机程序(包括程序代码)。需要说明的是，此处的计算机可读存储介质可以是高速ram存储器，也可以是非不稳定的存储器(non-volatile memory)，例如至少一个磁盘存储器。可由处理器加载并执行计算机可读存储介质中存放的一条或一条以上指令，以实现上述实施例中一种空间位置测量方法的步骤。
[0097]
本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。
[0098]
本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
[0099]
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
[0100]
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
[0101]
最后应当说明的是:以上实施例仅用于说明本发明的技术方案而非对其保护范围的限制,尽管参照上述实施例对本发明进行了详细的说明,所属领域的普通技术人员应当理解:本领域技术人员阅读本发明后依然可对申请的具体实施方式进行种种变更、修改或者等同替换，但这些变更、修改或者等同替换，均在申请待批的权利要求保护范围之内。

技术特征：
1.一种空间位置测量装置，其特征在于，包括至少两个连杆，相邻的所述连杆之间首尾转动连接；每一个所述连杆上均设置有一个陀螺仪，所述陀螺仪位于测量平面内，所述测量平面由全部所述连杆以及两个待测空间点所在直线围成；每一个所述陀螺仪分别与控制板通信连接，每一个所述陀螺仪分别与控制板电连接，所述控制板用于提供电源和数据处理。2.如权利要求1所述的装置，其特征在于，所述转动连接为铰接，相邻的所述连杆之间具有相匹配的铰接点(4)。3.如权利要求2所述的装置，其特征在于，所述陀螺仪与所述控制板之间通过导线(5)连接。4.一种空间位置测量方法，其特征在于，包括：将装置放置在待测空间点所在的原始坐标系下，使得装置中的陀螺仪中三轴坐标系与所述原始坐标系重合，得到每一个陀螺仪的初始姿态角并计算对应的旋转矩阵；将两个待测空间点与装置中首尾连杆的自由端相接触，分别获取每一个连杆上陀螺仪的最终姿态角并计算对应的旋转矩阵；根据所述初始姿态角对应的旋转矩阵、所述最终姿态角对应的旋转矩阵以及对应的连杆长度，计算两个待测空间点之间的空间位置关系矩阵；根据所述空间位置关系矩阵计算两个待测空间点之间的距离；其中，所述装置为权利要求1至3任一项所述的一种空间位置测量装置。5.如权利要求4所述的方法，其特征在于，所述计算两个待测空间点之间的空间位置关系矩阵，包括：根据每一个陀螺仪的初始姿态角对应的旋转矩阵和最终姿态角对应的旋转矩阵计算每一个陀螺仪对应的转换矩阵；根据每一个陀螺仪对应的转换矩阵和对应的连杆长度计算所述空间位置关系矩阵。6.如权利要求5所述的方法，其特征在于，所述根据每一个陀螺仪对应的转换矩阵和对应的连杆长度计算所述空间位置关系矩阵，通过以下计算式：(x,y,z)
t
＝r1(l1,0,0)
t
+r2(l2,0,0)
t
+...+r
n
(l
n
,0,0)
t
...+r
n
(l
n
,0,0)
t
其中，(x,y,z)
t
为所述空间位置关系矩阵，r
n
为第n个陀螺仪对应的转换矩阵，l
n
第n个陀螺仪所在连杆的长度，n为所述连杆的总数。7.如权利要求6所述的方法，其特征在于，所述根据每一个陀螺仪的初始姿态角对应的旋转矩阵和最终姿态角对应的旋转矩阵计算每一个陀螺仪对应的转换矩阵，通过以下计算式：r
n
＝r
n(2)-1
r
n(1)
其中，r
n(1)
为第n个陀螺仪的初始姿态角对应的旋转矩阵，r
n(2)
为第n个陀螺仪的最终姿态角对应的旋转矩阵。8.如权利要求6所述的方法，其特征在于，所述根据所述空间位置关系矩阵计算两个待测空间点之间的距离，通过以下计算式：
其中，d为两个待测空间点之间的距离，x、y、z分别为所述空间位置关系矩阵(x,y,z)
t
中的分量值。9.一种计算机设备，其特征在于，包括：一个或多个处理器；存储器，用于存储一个或多个程序；当所述一个或多个程序被所述一个或多个处理器执行时，实现权利要求4至8中任一项所述的一种空间位置测量方法。10.一种计算机可读存储介质，其特征在于，其上存有计算机程序，所述计算机程序被执行时，实现权利要求4至8中任一项所述的一种空间位置测量方法。

技术总结
本发明提供了一种空间位置测量装置与方法，该装置包括至少两个连杆，相邻的所述连杆之间首尾转动连接；每一个所述连杆上均设置有一个陀螺仪，所述陀螺仪位于测量平面内，所述测量平面由全部所述连杆以及两个待测空间点所在直线围成；每一个所述陀螺仪分别与控制板通信连接，每一个所述陀螺仪分别与控制板电连接，所述控制板用于提供电源和数据处理。本发明通过将多连杆的两端与待测量的两个点接触，读取每一个连杆上陀螺仪的姿态角数据，从而可以计算出空间两个点的距离及位置关系，进而实现了在两个点之间存在障碍物的时候，也可以完成两个点之间位置关系和距离的测量。成两个点之间位置关系和距离的测量。成两个点之间位置关系和距离的测量。


技术研发人员：刘梁 李由 王春慧 姜昌华 展文豪 谭丽芬 张小鹏
受保护的技术使用者：中国航天员科研训练中心
技术研发日：2023.04.27
技术公布日：2023/8/23