CN1148161A - 一种实时测量运动体位移姿态角的装置 - Google Patents

Abstract

一种实时测量运动物体位移和姿态角的装置，包括一组或多组光源及光源驱动器、一组或多组测量单元和一个信息处理单元，其特点在于：测量单元由选择光束或光面的裁光板和光照位移传感器组成，利用光照位移传感器对透过裁光板的光照位移量进行测量，信息处理单元根据光照位移量求解不同的光面方程，算出光源在空间的相对坐标，进而求出运动物体三个方向的位移量和三个姿态角。本发明具有实时性好、测量范围大、测量精度高、抗干扰能力强和结构重量轻的优点。

Description

一种实时测量运动物体位移和姿态角的装置

本发明涉及一种实时测量运动物体位移和姿态角的装置。由于许多运动物体的运动规律不确定，所处的环境复杂，自身负载能力有限，无法直接进行接触测量。选种情况下目前，主要采用机械扫描式和固体摄像式两种测量装置对运动物体的位移和姿态角进行测量。机械扫描式测量装置主要由三部分组成，光源部分、机械扫描部分和信号处理部分，光源部分主要包括控制电路和发光元件，机械扫描部分主要包括棱镜、电机和光电接收元件，信号处理部分主要由滤波电路、放大电路和单片机组成，其工作原理是将一组或多组光源固定在被测物体上，然后将至少两套机械扫描装置放置在距光源一定的距离，成一定的角度布置，控制光源发出频率相同而相位不同的光脉冲，电机驱动机械扫描装置转动，光电接收器件将光信号转换成电信号进到信息处理单元进行处理、解算，从而求出光源在空间的一维或二维坐标，并由此求出物体的两个姿态角。这种测量装置由于受机械扫描装置限制，实时性较差，测量精度低，且由于有电机存在，整个测量系统的结构重量较重，所以在实际使用中受到了限制；固体摄像式测量装置由光源、固体摄像机和信号处理部分组成，它也是将一组或多组光源固定在被测物体上，固体摄像机作为测量元件，将光信号变成电信号，再由信息处理部分进行象素处理、运算，从而求出光源坐标，再由光源坐标求出被测物体的位移和姿态角，这种测量装置要处理的信息量大，致使实时性比较差，测量精度由固体摄像机分辨率所限制，所以精度较低，而且摄像机本身较重，使得重量无法降低。

本发明的目的在于克服已有技术的不足，提供一种实时性好、测量精度高、抗干扰能力强，且结构重量轻的实时测量运动物体的位移和姿态角的装置。

本发明的目的是通过以下措施实现的：一种实时测量运动物体位移和姿态角的装置包括一组或多组光源及光源驱动器、一组或多组测量单元和一个信息处理单元，其特点在于：上述的测量单元由选择光束或光面的裁光板和光照位移传感器组成，上述的裁光板置于上述光源和上述光照位移传感器之间，并与上述的光照位移传感器平行放置。光源驱动器控制光源发出光脉冲，由光照位移传感器对透过裁光板的光照位移进行测量，信息处理单元根据测得的光照位移量确定不同的光面方程，求解相应的发光点的坐标，再由发光点的坐标求解运动物体的位移和姿态角。

本发明与现有技术相比具有如下优点：

1.由于采用裁光板和光照位移传感器作为测量元件，使得本发明可以使用红外光源作为光源，因此该装置对周围的电磁环境和照明环境干扰比较小，抗干扰能力也较强。

2.由于光照位移传感器的分辨率比较高，一般可这几微米量级，通过它可以取得高精度的光源坐标，进而获得高的位移和姿态角精度，因此本发明测量精度高。

3.由于本发明的测量范围可以通过缩短裁光板与光照位移传感器之间的距离而扩大，而且光照位移传感器的分辨率有较大的余度，因此本发明可以在同样精度条件下比机械扫描测量装置具有更大的测量范围。

4.由于本发明采用光照位移传感器对光照位移进行直接测量，没有机械环节和图像处理环节，所以本发明采样速度和数据处理的速度都比较高，比机械扫描式测量装置和固体摄像式测量的实时性都好，机械扫描式和固体摄像式的测量延迟一般大于30毫秒，本系统可使延迟时间控制在10毫秒以内。

5.由于裁光板和光照位移传感器都结构简单，质量较小，所以本发明总的结构重量较轻。

下面结合附图及实施例对本发明进一步详细说明。

图1为本发明的组成原理框图；

图2为本发明测量单元一种结构简图；

图3为本发明测量单元另一种结构简图；

图4为本发明信息处理单元原理框图；

图5为本发明信息处理单元电原理图；

图6为本发明解算光源坐标示意图；

图7为本发明实时测量程序框图。

参见附图1、2、3，固定在被测物体上的光源1采用红外点光源，几个发光点合称为一组，根据测量范围的大小可以选用多组光源，本实施例采用3个发光点为一组的光源，光源1随着被测物体在裁光板3即A′B′C′D′正前30公分处直径为半米的球内，即测量范围内运动。光源驱动器2控制光源1使其发出频率相同而相位不同的光脉冲，裁光板3和光照位移传感器4组成测量单元，并由光照位移传感器4对透过裁光板3的光照位移进行测量。裁光板3的作用是完成对光束或光面的选取，光照位移传感器4的作用是测定光照位移量，它是一种半导体器件，在接收光照信号后，两极输出电流，两极电流的偏差与光照位移成线性关系，与光强无关。裁光板3有两种设计结构，第一种是在薄的透光材料下面镀一层遮光膜，然后在遮光膜上刻画三条不全平行的透光缝6，参见附图2，透光缝6的长度根据测量范围大小来选择，如果测量范围大，需要光源投射的角度就大，那么透光缝6的长度就要长一些，一般取3-50毫米，透光缝6宽度可根据光强和精度选取，如果测量精度高，光强度高，那么透光缝6就要窄一些，一般在100-500微米之间选取，对于图2的裁光板需要采用一维光照位移传感器4来测量，该一维传感器4与裁光板平行放置，其敏感面对着裁光板，当一维传感器4的敏感面大小确定时，两者之间的距离00′与测量范围成反比，当测量范围确定时，两者之间的距离00′与光照位移传感器4的敏感面的长度成正比，即根据不同的测量范围要求和光照位移传感器敏感面的尺寸来选取裁光板与传感器之间的距离。当光源1发光时，三个透光缝6各取某一光面，投射于相应一维光照位移传感器敏感面上，一维光照位移传感器4即可测出沿相应方向上的光照位移。裁光板3的另一设计结构是在薄的透光材料下面镀一层遮光膜，在遮光膜上加工三个微型透光孔7，参见附图3，微型透光孔7的直径根据光强和精度要求在100-500微米中间选取，对于图2的裁光板需要采用二维光照位移传感器4′来测量，该二维光照位移传感器4′与裁光板也平行放置，其敏感面也需对着裁光板，两者之间的距离也是与测量范围成反比，与该二维光照位移传感器4′的敏感面尺寸成正比。当光源1发光时，微型透光孔7选取不同的光束线，投射于二维光照位移传感器4′的敏感面上，该二维光照位移传感器4′测出光束在敏感面上的光照位移量，之后再由信息处理单元5根据光照位移量确定三个不同的光面方程，再由光面方程实时解算出每个发光点的坐标，根据发光点的坐标再求出位移和姿态角。

参见附图4，信息处理单元5由信号提取电路和完成实时解算与控制系统的单片机系统组成。信号提取电路由放大电路51、求差电路52、求和电路53、求比电路54、采样保持电路55、A/D转换电路56、实时解算控制部件单片机系统57组成。光照位移传感器4输出的光照位移量经放大电路51放大后分两路，一路进入求差电路52，对放大了的光照位移信号进行求差运算，另一路进入求和电路53，对放大了的光照位移信号进行求和运算，求差电路52和求和电路53的输出经求比电路54进行求比运算后，使光照位移信号转化成具有一定精度的可分辨的模拟信号，该模拟信号由采样保持电路55、A/D转换电路56转换成数字信号后，送到实时解算控制单片机系统57中处理，完成实时计算，单片机系统57同时还控制光源驱动器2驱动光源1。

参见附图5，三个光照位移传感器4，测量的光照位移量首先经0P1-0P6运算放大对其进行放大，再由OP7、0P9、0P11对信号求和，由0P8、OP10、0P12对信号求差，AD734、AD7342、AD7343对求出的差与和信号求比，LP3981-LE3983对三个通道的光照位移信号实时进行采样保持，AD5741-AD5743则对相应的LF3981-LF3983输出的信号进行模数转换，转换完成后由8255统一输入单片机系统8098中，由8098、6264、2764、8255、74LS133、74LS138一起构成实时运算和控制模块。光源驱动电路2由MC1413和相应的发光管组成，最终的计算结果也由8255通过PA口输出。

参见附图6，解算光源坐标的具体过程如下：建立如图6所示的直角坐标系，假设采用加工有透光缝6的栽光板，且设：

w为透光缝6的长度，

l表示坐标原点到三个透光缝6的距离，

h表示透光缝6到光照位移传感器4敏感面的距离，

p1、p2、p3为三个光照位移传感器测得的光照位移量，

则有如下三个点同处于一个光面AP上，可表示为：

(l，w，h)，(l，-w，h)，(l+p1，0，0)；

同理在BP平面上也有三个点分别可表示为：

(w，l，h)，(-w，l，h)，(0，l+p2，0)；

在CP平面上有三个点分别可表示为：

(w，-l，h)，(-w，-l，h)，(O，-l+p3，0)，

因为标准的平面方程为：  AX+BY+CZ＝D

将AP平面上的三个点的坐标分别代入其中得：

解得：B＝0，C＝p1Ah，D＝(l+p1)A

故，X+p1Z/h＝l+P1       (1)

同理BP平面有：

  Y+p2Z/h＝l+p2       (2)

CP平面有：

Y+p1Z/h＝-l+p3      (3)解(1)、(2)、(3)组成的联立方程得：

(4)就是光源坐标算法表达式，按照该表达式可以求出一组中三个不同的光源坐标。

根据上述三个光源坐标即可求出运动物体的位移和姿态角，具体过程如下：

假设上述一组光源的三个发光点，相对距离为10公分，解出的坐标为：

Q1(x1，y1，z1)、Q2(x2，y2，z2)和Q3(x3，y3，z3)，

设01为Q1Q2的中点，则有

O1((x1+x2)/2，(y1+y2)/2，(z1+z2)/2)

则向量Q1Q2的表达式为：

(x2-x1，y2-y1，z2-z1)，

向量O1Q3的表达式为：

(x3-(x1+x2)/2，y3-(y1+y2)，

z3-(z1+z2)/2)，

由向量投影的基本原理得Q1Q2沿Z轴的投影为：

|Q1Q2|z＝z2-z1

所以，运动物体沿水平方向上的转动角度为：

DA＝arcsin(|Q1Q2|z/|Q1Q2|)-a0

＝arcsin((z2-z1)/10)-a0

同理可得：|Q1Q3|z＝z3(z1+z2)/2，

|Q1Q3|x＝x3-(x2-x1)/2

DB＝arcsin(|Q1Q|z/Q1Q3|)-b0

＝arcsin((z3-(z2+z1)/2/0.866×10)-b0

DR＝arcsin(|Q1Q3|x/|Q1Q3|)-r0

＝arcsin((x3-(x2+x1)/2)/0.866×10)-r0

则DA、DB和DR就是所求的运动物体的姿态角，a0、b0和r0为初始姿态角。

对于位移的求解过程如下：因为

Q1Q2＝(x2-x1，y2-y1，z2-z1)，

Q1Q3＝(x3-x1，y3-y1，z3-z1)，

根据向量积的运算可知其法向向量n为 $\left[\begin{array}{ccc}i& j& k\\ x2-x1& y2-y1& z2-z1\\ x3-x1& y3-y1& z3-z1\end{array}\right]$ ＝((y2-y1)(z3-z1)-(y3-y1)(z2-z1)i((z2-z1)(z3-z1)-(x3-x1)(z2-z1)j((x2-x1)(y3-y1)-(x3-x1)(y2-y1)k＝Nxi+Nyj+Nzk

同样由向量积的运算法则可知它的模为0.866×10²

由此可知运动物体的位移DX、DY和DZ分别为

DZ＝(x1+x2+x3)/3-m×Nx/0.8660×10²-x0

DY＝(y1+y2+y3)/3-m×Ny/0.866×10²-y0

DX＝(z1+z2+z3)/3-m×Nz/0.866×10²-z0

这就是所求的运动物体位移表达式。上式中的x0、y0、z0为初始X、Y、Z值，m为运动物体中心到光源中心的距离。

本发明是一种实时测量装置，在装置工作时，可以对位移和姿态角进行连续实时的检测，实际工作程序参见附图6，首先进行系统初始化，对于单片机和环境参数进行设置，接着单片机系统控制光源驱动器驱动光源发光，光脉冲形成的光照位移信号由光照位移传感器接收，单片控制开始采样，采样完成后开始保持，并启动AD转换电路完成转换，控制8255开始读取数据并存贮，由8098CPU进行数据预处理，按上面所述方法求解光源坐标，进一步求出被测对象的位移和姿态角，一面将位移和姿态角输出，一面根据姿态角的范围确定下一次应对哪一组光源进行测量，确定后即控制光源发光，又重复上述过程，这样即可反复进行实时测量。

本发明因为具有上述优点，在飞行模拟器、飞机或坦克头盔瞄准具上拥有广泛的应用前景。下面以飞行模拟器头位跟踪系统中的应用情况为例说明其使用情况。将光源固定在驾驶员头盔上，将测量单元放在座舱上光源可照到的一侧，信息处理单元放在座舱附近，设置好初始参数，启动本装置工作，则本装置可实时高速(延迟不超过10毫秒)将驾驶员的头部位移和姿态角送给主控计算机，用此信号可对视景进行切换控制和对要攻击的目标进行跟踪攻击。

Claims (4)

1.一种实时测量运动物体位移和姿态角的装置，它包括一组或多组光源及光源驱动器、一组或多组测量单元和一个信息处理单元，其特征在于：上述的测量单元由选择光束或光面的裁光板和光照位移传感器组成，上述的裁光板置于上述光源和上述光照位移传感器之间，并与光照位移传感器平行放置。

2.根据权利要求1所述的测量装置，其特征在于：上述裁光板由薄的透光材料及在其下面镀的一层遮光膜组成，并在遮光膜上记刻画三条不全平行的透光缝或在遮光膜上加工出三个微型透光孔。

3.根据权利要求1所述的测量装置，其特征在于：光照位移传感器敏感面长度确定时，裁光板与光照位移传感器之间的距离与测量范围成反比，测量范围确定时，裁光板与光照位移传感器之间的距离与光照位移传感器敏感面的长度成正比。

4.根据权利要求1所述的测量装置，其特征在于：信号处理单元由信号提取电路和实时解算和控制系统的单片机系统组成，信号提取电路由放大电路、求和电路、求差电路、求比电路、采样保持及A/D转换电路组成，光照位移传感器测得的位移量经放大电路后分两路，一路进入求和电路，另一路进入求差电路，求和电路和求差电路的输出进入求比电路，求比电路的输出经采样保持及A/D电路后进入单片机系统。

Images