基于线阵CCD的空间目标姿态测量系统及其测量方法

摘要

本发明提供了一种基于线阵CCD的空间目标姿态测量系统及其方法，它是一种采用基于9个含有滤光片的选择性一维成像单元组合的3个三维坐标重构子系统分别实时重构按一定规则布置于被测物体上携带其姿态信息且与其所含滤光片波长范围一一对应的3个点合作目标的空间三维坐标，通过空间解算确定被测物体姿态角的方法，相应提出了由上述组合装置、各一维成像单元控制板、主控板、扩展LCD和键盘的多CPU并行的流水线式的专用嵌入式系统。本发明克服了采用面阵图像传感器作为信息输入设备用于空间目标姿态测量时存在的速度和精度的矛盾，易于满足空间运动目标姿态测量实时性的要求，使用方便。

说明书

(一)技术领域

本发明涉及光电式几何量测量技术领域，具体涉及一种空间目标姿态测量方法。

(二)背景技术

实现对空间目标的姿态测量也就是对其姿态角(俯仰角α、偏航角β和滚动角γ)的测量，可以广泛应用于机器人研究、柔性制造系统、精密机械装配及含有内部姿态敏感装置飞行器的出厂检验等领域。目前主要采用两种方式：一种是基于激光干涉技术的姿态测量方式，代表产品为API的第三代激光跟踪仪，可以实现对运动目标高精度的姿态测量，但合作目标复杂，适用性差，不易于实现动态测量，而且价格昂贵；另一种是基于机器视觉的姿态测量方式，根据是否在被测物体上安装合作目标，或是采用摄像机的数量，姿态测量模型各不相同，但均采用基于面阵图像传感器的摄像机进行测量，多存在速度和精度的矛盾。

(三)发明内容

本发明的目的在于提供一种技术、结构简单，成本低，在同等测量精度的前提下，其测量范围可以做的很大，且易于满足实时测量要求的基于线阵CCD的空间目标姿态测量系统及其测量方法。

本发明的目的是这样实现的：

本发明提出的基于线阵CCD的空间目标姿态测量系统包括一路双点目标三维坐标重构子系统S3DCRDPT、一路点目标三维坐标重构子系统S3DCRPT及布置于被测物体上的且与相应坐标重构光学子系统中滤光片波长相对应的3个LED作为PCT的姿态测量装置和9路并行的含有CPLD单元、线阵CCD单元、抗串扰的高速模/数(A/D)转换单元、FIFO存储单元、分ARM单元的1DIUS控制板对应1个含有3个从ARM单元和1个主ARM单元的主控板的多CPU并行的流水线式的实时信息采集和处理的硬件结构、时钟和电源。

本发明的基于线阵CCD的空间目标姿态测量系统还包括这样一些技术特征：

1、所述双点目标三维坐标重构子系统S3DCRDPT是由三个双点目标一维成像子系统1DISDPT彼此光轴成一定交角且在三个等间距位置上固定组成；

2、所述双点目标一维成像子系统1DISDPT由一个分光棱镜及共轭位置的两个含有不同波长范围的选择性一维成像单元1DIUS组成；

3、所述选择性一维成像单元1DIUS由一个滤光片、一个柱面镜头及位于镜头焦平面上且与其轴心线垂直的一个线阵CCD构成；

4、所述点目标三维坐标重构子系统S3DCRPT是由三个选择性一维成像单元1DIUS彼此光轴成一定交角且在三个等间距位置上固定组成；

5、所述硬件结构为采用AT91RM9200为主ARM单元，扩展键盘、LCD单元、SD卡单元、USB单元、SDRAM单元、FLASH单元、JTAG单元、RS232外设和接口的专用嵌入式结构；

6、所述9路1DIUS控制板上的线阵CCD单元、A/D转换单元及FIFO存储单元均连接CPLD单元，CPLD单元连接主控板的主ARM单元。

本发明提出的基于线阵CCD的空间目标姿态测量方法为：首先采用一组由一个滤光片、一个柱面镜头及位于镜头焦平面上且与其轴心线垂直的一个线阵CCD构成的选择性一维成像单元1DIUS组合而成的空间目标姿态测量系统，即采用三个由一个分光棱镜及共轭位置的两个含有不同波长范围的选择性一维成像单元1DIUS组成的双点目标一维成像子系统1DISDPT彼此光轴成一定交角且在三个等间距位置上固定而成的双点目标三维坐标重构子系统S3DCRDPT和三个选择性一维成像单元1DIUS彼此光轴成一定交角且在三个等间距位置上固定而成的点目标三维坐标重构子系统S3DCRPT，分别实时重构按一定规则布置于被测物体上携带其姿态信息且分别与重构光学子系统中滤光片波长范围一一对应的两个和一个点合作目标(PCT)的三维坐标，再经过实时空间解算，确定被测物体姿态角，它依次含有以下步骤：

(1)标定1DIUS的7个l参数；

(2)布置PCT；

(3)1DIUS分别实时捕捉被测物体上与其相对应的PCT的像坐标；

(4)采用7系数直接变换(DLT)法重构各PCT的空间三维坐标；

(5)采用空间解算求解被测物体的姿态角(俯仰角α、偏航角β和滚动角γ)；

(6)判断测量是否结束：若否，则返回步骤(3)；若是，则结束测量。

本发明的基于线阵CCD的空间目标姿态测量方法还包括这样一些技术特征：

1、所述的标定1DIUS的7个l参数依次含有如下步骤：

(1)在含有滑动标尺座的平面校准模板上均匀布置x、y坐标已知，且与PCT相同的3种标志点，在纵深方向调解滑动标尺给出z坐标，实现测量空间内标志点的均匀布置；

(2)分别用S3DCRDPT和S3DCRPT中的1DIUS捕捉对应波长范围标志点的像坐标；

(3)根据多于7个的测量空间内均匀布置的标志点已知的三维坐标和它们对应的一维像坐标，采用最小二乘法求解二者所确定平面方程7个l系数的最优解。

2、所述的布置PCT依次含有如下步骤：

(1)取被测物体轴心线与两端面的交点分别布置两个波长范围不同的LED；

(2)在其中一个端面内过交点且与被测物体轴心线垂直的直线上远离该交点的一点处布置另一波长范围的LED。

3、所述的1DIUS分别实时捕捉被测物体上与其相对应的PCT的像坐标依次含有如下步骤：

(1)主ARM驱动LED控制板点亮放置于被测物体上的PCT；

(2)主ARM输出一路可编程时钟分为9路分送9个1DIUS控制板上的CPLD以产生该路CCD的驱动时序、A/D的采样时序及FIFO的写时序等；

(3)主ARM驱动各从ARM，各从ARM驱动相应分ARM，均进入姿态测量程序；

(4)各线阵CCD分别实时捕捉放置于被测物体上携带其姿态信息且与其所在1DIUS相对应的PCT；

(5)各分ARM将对A/D转换的帧数据处理后的一维像点坐标传给相应从ARM；

(6)各分ARM判断姿态测量是否结束：若否，则返回步骤(4)；若是，则结束。

4、所述的采用7系数DLT法重构各PCT的空间三维坐标是通过以下方法实现的：

(1)3个从ARM分别对应3个分ARM接收3个1DIUS获取的对应PCT当前时刻的一维像点坐标λ₁₁、λ₁₂和λ₁₃；

(2)在每个从ARM内，根据上述求得的每3个1DIUS固有参数的最小二乘解l₁^*、l₂^*和l₃^*及每个1DIUS当前时刻实时获取的一维像点坐标联立求解当前时刻对应PCT的空间三维坐标

$\left(\begin{array}{ccc}{l}_{11}^{*}-{\lambda }_{11}{l}_{15}^{*}& l_{12}^{*}-{\lambda }_{11}{l}_{16}^{*}& l_{13}^{*}-{\lambda }_{11}{l}_{17}^{*}\\ l_{21}^{*}-{\lambda }_{12}{l}_{25}^{*}& l_{22}^{*}-{\lambda }_{12}{l}_{26}^{*}& l_{23}^{*}-{\lambda }_{12}{l}_{27}^{*}\\ l_{31}^{*}-{\lambda }_{13}{l}_{35}^{*}& l_{32}^{*}-{\lambda }_{13}{l}_{36}^{*}& l_{33}^{*}-{\lambda }_{13}{\lambda }_{37}^{*}\end{array},\right)·\left(\begin{array}{c}x\\ y\\ z\end{array}\right)=\left(\begin{array}{c}{\lambda }_{11}-l_{14}^{*}\\ {\lambda }_{12}-l_{24}^{*}\\ {\lambda }_{13}-l_{34}^{*}\end{array}\right).$

5、所述的采用空间解算求解被测物体的姿态角，包括俯仰角α、偏航角β和滚动角γ是通过以下方法实现的：

(1)3个从ARM分别将各对应PCTa、PCTb和PCTc的三维坐标(x_a，y_a，z_a)，(x_b，y_b，z_b)和(x_c，y_c，z_c)分传给主ARM；

(2)主ARM通过如下空间解算确定被测物体姿态角，

$\alpha =\arcsin \left(\frac{y_b-y_a}{l_{\mathrm{ab}}}\right),$

$\beta =\arcsin \left(\frac{z_b-z_a}{l_{\mathrm{ab}}}\right),$

$\gamma =\arcsin \left(\frac{(y_c-y_b)·l_{\mathrm{ab}}}{\sqrt{{\left(l_{\mathrm{ab}}\right)}^2-{(y_b-y_a)}^2}·l_{\mathrm{ac}}}\right).$

其中，PCTb和PCTc位于被测物体的同一端面，且

$l_{\mathrm{ab}}=\sqrt{{(x_a-x_b)}^2+{(y_a-y_b)}^2+{(z_a-z_b)}^2},$

$l_{\mathrm{ac}}=\sqrt{{(x_a-x_c)}^2+{(y_a-y_c)}^2+{(z_a-z_c)}^2}.$

本发明基于线阵CCD的空间目标姿态测量方法就是采用一路双点目标三维坐标重构子系统(subsystems for 3D coordinate reconstruction of double point target，简称S3DCRDPT)和一路点目标三维坐标重构子系统(subsystem for 3D coordinate reconstruction of pointtarget，简称S3DCRPT)分别实时重构按一定规则布置于被测物体上的两个和一个波长范围均不相同的点合作目标(point cooperation target，简称PCT)的三维坐标，通过实时空间解算确定被测物体的姿态角。含有滤光片2、柱面镜头3和位于其焦平面且与其轴心线垂直的线阵CCD4构成的选择性一维成像单元(1D imaging unit with selectivity，简称1DIUS)如图1所示，它可以检测远处与其滤光片波长范围相对应的光点1在垂直于柱面镜头轴心线方向上的一维运动，5为光点的线状像斑。非共轴、非平行的3个含有相同波长范围滤光片的1DIUS，引出三个平面相交于一点，可以唯一确定远处相应波长范围光点的空间位置，我们称其为S3DCRPT，如图2所示。图3为双点目标一维成像子系统(1D imaging subsystemofdouble point targets，简称1DISDPT)，它借助于一个分光棱镜6，和共轭位置的两个含有不同波长范围滤光片的1DIUS，实现对远处两个相应波长范围光点的同时捕捉；3个非共轴、非平行的1DISDPT，引出六个平面相交于两点，构成S3DCRDPT，可以同时实现对两个光点的三维定位，它相当于两个S3DCRPT的组合。运动中不产生形变的物体上的三个点可以确定其姿态角，取被测物体轴心线与其端面的两个交点及在其中一个端面内过交点且与其轴心线垂直的直线上远离该交点的一点处分别布置3个不同波长范围的发光二极管(LED)作为PCT，它们分别与含有相应波长范围滤光片的坐标重构子系统相呼应，通过实时的坐标重构和空间解算，则被测物体的姿态角可求。

本发明系统的主要特点为：

1.该姿态测量系统是多个基于线阵CCD单元的选择性一维成像单元(1DIUS)的组合，克服了采用面阵CCD作为信息输入设备时存在的速度和精度的矛盾；

2.所设计的双点目标三维坐标重构子系统(S3DCRDPT)相当于两个点目标三维坐标重构子系统(S3DCRPT)的组合，提高测量精度的同时节省了测量空间；

3.按一定规则布置于被测物体之上携带其姿态变化信息的PCT分别与相应坐标重构光学子系统中滤光片的波长范围一一对应，解决了多PCT与多成像单元间的目标干扰问题；

4.线阵CCD单元采用东芝公司的线阵CCD TCD1708D，它在拥有高分辨率的同时还降低了对光学镜头尺寸的要求，且PCT和滤光片的波长范围与它的光谱敏感区间相对应；

5.设计了带有滑动标尺座的平面校准模板，通过依次点亮各标志点(给出x，y坐标)及调节滑动标尺在纵深方向(给出z坐标)的变化，方便实现在测量空间内均匀布置高密度的标志点；

6.各1DIUS和各1DISDPT光轴间的夹角及位置根据测量空间和测量精度要求等因素被综合考虑并固定，校准一次后，不需要再重新校准，仅当测量空间改变时，需要重新考虑、调整固定和校准；

7.采用9个分ARM单元、3个从ARM单元、1个主ARM单元的多CPU并行的流水线式的硬件结构，有利于数据的实时处理；

8.选用AT91RM9200为主ARM单元，扩展键盘、LCD单元、SD卡单元、USB单元、SDRAM单元、FLASH单元、JTAG单元、RS232外设和接口的专用嵌入式结构，提高了系统的工作效率。

本发明克服了采用面阵图像传感器作为信息输入设备用于空间目标姿态测量时存在的速度和精度的矛盾，在同等测量精度的前提下，其测量范围可以做的很大，且易于满足空间运动目标姿态测量实时性的要求，使用方便。实验证明，本发明测量系统可以对运动的空间目标的姿态进行方便的测量，且具有很好的实时性。

(四)附图说明

图1为选择性一维成像单元(1DIUS)结构和成像示意图；

图2为点目标三维坐标重构子系统(S3DCRPT)结构示意图；

图3为双点目标一维成像子系统(1DISDPT)结构示意图；

图4为本发明提出的空间目标姿态测量系统装置示意图；

图5为双点目标一维成像子系统(1DISDPT)的简化结构示意图；

图6为带有滑动标尺座的平面校准模板示意图；

图7为本发明提出的空间目标姿态测量系统硬件结构框图；

图8为基于线阵CCD的空间目标姿态测量方法的主控程序流程框图。

(五)具体实施方式

下面结合附图对本发明作进一步的说明：

针对一个圆柱体被测对象，根据基于线阵CCD的姿态测量原理，设计出典型的姿态测量装置如图4所示。取被测物体轴心线与两端面的交点及过第二个交点的圆端面半径上的非圆心端点处分别布置可见光波段内的波长范围分别为580～595nm、615～635nm和515～530nm的LED作为PCTa7、PCTb8和PCTc9，初始时刻，该三点确定的平面应与水平面平行；3个1DISDPT按照光轴方向成30°交角在等间距的位置上固定，构成S3DCRDPT10；3个1DIUS按照光轴方向成30°交角在等间距的位置上固定，构成S3DCRPT11，这相当于3个S3DCRPT或9个1DIUS的一个组合；上述装置中，1DIUS中滤光片与不同波长范围PCT的对应，解决了多三维坐标重构子系统与多PCT一一对应时的目标干扰问题；位于S3DCRDPT中间的1DISDPT和位于S3DCRPT中间的1DIUS的主光轴分别垂直于被测物体运动空间的前端面，S3DCRDPT这种新光学构架的设计，提高了测量精度，节省了测量空间，当焦距与光学镜头的其它特性参数相协调且不会影响成像质量时，可以采用图5所示的结构，即把图3中共轭位置的两个1DIUS进一步合并，二者共用一个柱面镜头。

设计光学镜头的分辨率大于线阵CCD的分辨率，则1DIUS的分辨率由CCD的分辨率确定。因此，像元大小为4.7μm，有效像元数为7450，且在可见光波段范围内反应均敏感的东芝公司的TCD1708D被选择，它在拥有高分辨率的同时还降低了对光学镜头尺寸的要求。

对放置于被测物体上且携带其姿态变化信息的LED的选择是一个关键问题。根据CCD的光敏范围及布置原则确定3种不同波长范围的LED，它们既要对应CCD光谱反应的敏感区间又要有一定的光谱间隔以避免各LED与对应坐标重构子系统中滤光片波长范围一一对应时彼此间的相互干扰；LED的发光位置体现的是点位置，它的管芯要尽量小，发光功率要足够高，但为了保证特征点的提取精度，要求其在线阵CCD上的像点至少大于7个像元；同时，为了避免目标脱靶，它们还应具有较大的发散角，此装置中LED的发散角为120°。

为了让在1DIUS视场内运动的PCT的一维位置信息被线阵CCD捕捉，应该选择含有柱面透镜的镜头，又因为光学镜头的理论分辨率与相对孔径成正比，而双高斯物镜是目前大孔径照相物镜的基础。因此，首先根据测量空间、测量精度和所选线阵CCD的有效感光面的长度确定各一维成像单元镜头的光学特性参数，然后从ZEMAX软件的镜头库中找出与所需特性参数相近的镜头结构，用一片或多片柱面透镜对其中的球透镜单元做替换，再根据光线追迹结果做优化，从而确定各柱面镜头的结构参数。

在基于机器视觉实现对空间运动目标的动态测量系统中，校准是一个很重要的环节。对应本发明提出的姿态测量系统，发明人设计了带有滑动标尺座的、可拆卸的平面校准模板，它上面均匀布置x、y坐标已知，且与PCT相同的3种标志点，通过调节滑动标尺给出z坐标，从而方便实现测量空间内高密度标志点的均匀布置，见图6。校准计算时，滑动标尺给出的标志点的z坐标与其真实值的补偿系数(设计模版时已知)需要被考虑。

与上述测量装置对应的结构框图如图7所示，它体现两个方向的信号走向。一方面，主控板上的主ARM的可编程时钟信号分成9路经LVDS传输，送入各1DIUS控制板上的CPLD以为该路线阵CCD、A/D和FIFO提供时序信号，而控制信号则是用户通过键盘与主ARM进行对话产生，而后主ARM驱动各从ARM，从ARM驱动相应分ARM；另一方面，体现了多CPU响应控制命令并行处理数据的走向，该系统采用9个基于线阵CCD的1DIUS(3个1DIUS对应1个PCT)实时进行图像采集，9路空间上离散的模拟信号分别经前端信号调理电路、抗串扰的高速MD电路、FIFO数据存储器，送入9个分ARM单元进行处理；所得PCT的一维像坐标经并/串转换和LVDS传输后，3个一组分别送主控板上的从ARM1、从ARM2和从ARM3，它们分别联立方程组求解3个PCT的空间三维坐标；3路三维坐标再经SPI串行传输后送总ARM，经过空间解算以确定当前时刻的姿态角，结果被存储并送往LCD显示终端。

基于线阵CCD的姿态测量方法的主控程序流程框图如图8所示，主ARM响应键盘输入的命令相应进入各程序：1)校准程序，首先，主ARM根据当前键盘子命令驱动LED控制板点亮校准模板上与S3DCRDPT或S3DCRPT对应的第一对或第一个标志点；然后，输出可编程时钟分送相应CPLD，以产生所在1DIUS控制板所需时钟信号；而后，驱动相应从ARM进入校准程序并等待其响应，判断采集的标志点是否是当前z坐标下对应校准模板上的最后一对或一个，若是，则等待移动校准模板滑动标尺座给出标志点新的z坐标后的键盘子命令ready，给出next信号驱动LED控制板熄灭校准模板上当前标志点并点亮对应的下一对或下一个标志点，若否，直接给出next信号；最后，判断是否有键盘结束命令？若否，重新启动一次校准，若是，则转向结束程序。2)姿测程序，首先，主ARM驱动LED控制板点亮布置于被测物体上的PCT，然后，输出可编程时钟分送各路CPLD，以产生各路1DIUS控制板所需时钟信号；而后，驱动各从ARM进入姿测程序并等待接收当前时刻三个PCT的三维坐标以进行姿态角的解算，结果被存储后送LCD显示；最后，判断是否有键盘结束命令？若否，等待依次接收下一时刻PCT的三维坐标，若是，则转向结束程序。3)结束程序，首先，驱动各从ARM进入结束程序；然后，驱动LED控制板进入结束程序；最后退出主控程序。

对于所设计的小视场1DIUS的光学镜头，采用基于针孔模型的7系数DLT法，则被测PCT的空间三维坐标(x，y，z)与它们相应线状像和线阵CCD的交点坐标λ之间的关系可以用下面的平面方程进行描述：

l₁x+l₂y+l₃z+l₄-λl₅x-λl₆y-λl₇z＝λ.    (1)

其中，l_i(i＝1，2，...7)为该平面方程的系数，也即1DIUS的固有参数，根据被测物体运动空间范围内均匀布置的n(n≥7)个标志点的三维坐标(x，y，z)和它们相应的一维像坐标，采用最小二程法求解这7个系数，表达式如下：

$\left(\begin{array}{ccccccc}{x}^{\left(1\right)}& y^{\left(1\right)}& z^{\left(1\right)}& 1& {-\lambda }^{\left(1\right)}{x}^{\left(1\right)}& {-\lambda }^{\left(1\right)}{y}^{\left(1\right)}& {-\lambda }^{\left(1\right)}{z}^{\left(1\right)}\\ x^{\left(2\right)}& y^{\left(2\right)}& z^{\left(2\right)}& 1& {-\lambda }^{\left(2\right)}{x}^{\left(2\right)}& {-\lambda }^{\left(2\right)}{y}^{\left(2\right)}& {-\lambda }^{\left(2\right)}{z}^{\left(2\right)}\\ x^{\left(3\right)}& y^{\left(3\right)}& z^{\left(3\right)}& 1& {-\lambda }^{\left(3\right)}{x}^{\left(3\right)}& {-\lambda }^{\left(3\right)}{y}^{\left(3\right)}& {-\lambda }^{\left(3\right)}{z}^{\left(3\right)}\\ & & & ·& & & \\ & & & ·& & & \\ & & & ·& & & \\ x^{\left(n\right)}& y^{\left(n\right)}& z^{\left(n\right)}& 1& {-\lambda }^{\left(n\right)}{x}^{\left(n\right)}& {-\lambda }^{\left(n\right)}{y}^{\left(n\right)}& {-\lambda }^{\left(n\right)}{z}^{\left(n\right)}\end{array}\right)·\left(\begin{array}{c}{l}_1\\ l_2\\ l_3\\ l_4\\ l_5\\ l_6\\ l_7\end{array}\right)=\left(\begin{array}{c}{\lambda }^{\left(1\right)}\\ {\lambda }^{\left(2\right)}\\ {\lambda }^{\left(3\right)}\\ {\lambda }^{\left(4\right)}\\ {\lambda }^{\left(5\right)}\\ {\lambda }^{\left(6\right)}\\ {\lambda }^{\left(7\right)}\end{array}\right).---\left(2\right)$

求解上述方程组系数矩阵的广义逆矩阵，则该方程组的最小二乘解l^*可求。假定构成各坐标重构子系统的3个1DIUS的最小二乘解分别为l₁^*、l₂^*和l₃^*，依次带入平面方程(1)整理联立得：

$\left(\begin{array}{ccc}{l}_{11}^{*}-{\lambda }_{11}{l}_{15}^{*}& l_{12}^{*}-{\lambda }_{11}{l}_{16}^{*}& l_{13}^{*}-{\lambda }_{11}{l}_{17}^{*}\\ l_{21}^{*}-{\lambda }_{12}{l}_{25}^{*}& l_{22}^{*}-{\lambda }_{12}{l}_{26}^{*}& l_{23}^{*}-{\lambda }_{12}{l}_{27}^{*}\\ l_{31}^{*}-{\lambda }_{13}{l}_{35}^{*}& l_{32}^{*}-{\lambda }_{13}{l}_{36}^{*}& l_{33}^{*}-{\lambda }_{13}{\lambda }_{37}^{*}\end{array}\right)·\left(\begin{array}{c}x\\ y\\ z\end{array}\right)=\left(\begin{array}{c}{\lambda }_{11}-l_{14}^{*}\\ {\lambda }_{12}-l_{24}^{*}\\ {\lambda }_{13}-l_{34}^{*}\end{array}\right).---\left(3\right)$

代入各坐标重构子系统3个1DIUS上对应PCT的一维像点坐标，则该PCT的空间三维坐标可求。假定PCTa、PCTb和PCTc当前时刻的坐标被分别重构为(x_a，y_a，z_a)，(x_b，y_b，z_b)和(x_c，y_c，z_c)，则根据空间几何关系，该被测物体当前时刻的俯仰角α、偏航角β和滚动角γ可以用下式求解：

$\alpha =\arcsin \left(\frac{y_b-y_a}{l_{\mathrm{ab}}}\right),---\left(4\right)$

$\beta =\arcsin \left(\frac{z_b-z_a}{l_{\mathrm{ab}}}\right),---\left(5\right)$

$\gamma =\arcsin \left(\frac{(y_c-y_b)·l_{\mathrm{ab}}}{\sqrt{{\left(l_{\mathrm{ab}}\right)}^2-{(y_b-y_a)}^2}·l_{\mathrm{ac}}}\right).---\left(6\right)$

其中，

$l_{\mathrm{ab}}=\sqrt{{(x_a-x_b)}^2+{(y_a-y_b)}^2+{(z_a-z_b)}^2},$

$l_{\mathrm{ac}}=\sqrt{{(x_a-x_c)}^2+{(y_a-y_c)}^2+{(z_a-z_c)}^2}.$

综上所述，该姿态测量系统的工作步骤如下：

首先，根据该被测物体的运动空间范围和姿态测量精度的要求确定、调整并固定好各1DISDPT和各1DIUS间的夹角；

其次，通过键盘启动校准程序，主ARM驱动LED控制板依次点亮平面校准模板上的与PCTa或PCTb和PCTc相同的标志点，并通过调节其滑动标尺座在纵深方向的位移，在测量空间内均匀布置标志点，分别校准构成S3DCRDPT或S3DCRPT的各1DIUS单元，所得各1DIUS的固有参数(7个l系数)将保存在主控板上相应的从ARM中；

再次，通过键盘启动姿测程序，主ARM驱动LED控制板点亮布置于被测物体上的PCTa、PCTb和PCTc，则9路线阵CCD分别实时捕捉与它们对应的PCT，各分ARM将处理后的一维像点坐标信息经并/串转换，LVDS传输给主控板上的3个从ARM，联立方程求解后的PCT的三维空间坐标再经SPI串行传输给主ARM，进行空间解算，则姿态角可求，并且当前时刻及姿态角被送往LCD显示终端输出。