。
[0101] 步骤12,旋转步骤11安装有棋盘格标定板的手动精密转台，并保持。
[0102] 步骤13，让双目立体视觉系统中的两台工业相机对步骤12中的棋盘格标定板进行 成像，并保存记录图像，如图5所示。
[0103] 步骤14,重复步骤12、13至少10次。
[0104] 步骤15,利用Matlab中的相机标定工具箱对步骤14重复得到的保存在2个相机中 的图片进行标定处理，也可以将该相机标定工具箱嵌入到导航计算机系统内，计算出双目 立体视觉系统的内外参数，如表1、表2所示。
[0105] 表1左相机标定参数
[0107]表2右相机标定参数
[0109] 同时可以获取表示两个相机姿态转换关系的罗德里格参数为[0.00643,0.29278， 0.01078]，对应的不确定度为[0.00836，0·00734,0· 00175]，平移参数为[-302.14941，_ 0.45862,29.49918]mm，对应的不确定度为[1.50777，0.52910,7.57241 ]mm，立体视觉测量 坐标系就是左相机的相机坐标系。
[0110] 步骤16,将上述参数导入到导航算法模块。
[0111] 步骤2,通过仿真控制计算机复位三轴电动转台，让其姿态处于标称零位。
[0112] 步骤3,通过仿真控制计算机复位三维电动平移台，让其位置处于标称零位。
[0113] 步骤4,导航计算机根据标定的内外参数和故障航天器等效装置上的LED灯阵确定 故障航天器等效装置与双目立体视觉系统的相对姿态的初值和相对位置的初值。具体包括 以下步骤。
[0114]步骤41，点亮在故障航天器等效装置上的LED灯阵，形成明显的标定特征点阵。
[0115] 步骤42,利用双目立体视觉系统对故障航天器等效装置表面的标定特征点阵成 像，提取特征点，在利用三维重建技术计算出每个特征点的坐标值。
[0116] 将2台工业相机分为左右相机，LED灯阵在右相机坐标系下的坐标为（XR^ym ZR,〇，在左相机坐标系下的坐标为(XL,uZL,〇，经过图像畸形矫正处理后，在左右相机 像平面的坐标分别为(XlaYw)和(Xr,^Yr,0。根据双目立体视觉系统标定的左右相机的有 效焦距(a Lx，aLy)和(aRx，aRy)，右相机相对测量坐标系的旋转矩阵R和平移矩阵T，得到该特征 点在测量坐标系下的三维坐标：
[0118]其中，（Xl，yi，Z1)为第i个特征点在测量坐标系下的坐标，
[0119]步骤43,利用测量得到的特征点的坐标值计算各个特征点之间的距离并求和，得 到的和值与故障航天器等效装置表面上对应的两个LED灯之间的已知距离进行比较，如果 该比较值大于设定的门限，则说明平台的实验精度不够，返回步骤11)。如果该比较值小于 一定门限，则进入后续实验步骤。
[0120] 比较方法：
[0121] 步骤431，利用测量坐标系下测量得到的特征点坐标值计算各个特征点之间的距 离并求和：
[0123] 其中，d为测量坐标系下测量得到的各个特征点之间的距离之和，（Xl，yi，Zl)为第i 个特征点在测量坐标系下的坐标，
[0124] 步骤432,根据步骤431得到的测量坐标系下测量得到的各个特征点之间的距离之 和和与故障航天器等效装置表面上对应的两个LED灯之间的距离之和d r比较:如果
[0125] dr-dI < ε 0
[0126] 其中，ε是设定的门限。则认为实验平台误差较小，认为双目视觉标定和初始相对 位姿的确定比较精确，可以满足后续实验要求。如果不满足要求，则返回步骤11。相应的距 离和的真值记为d r，该值由安装LED灯的机械尺寸决定，可以作为真值。
[0127] 步骤44,根据步骤43得到的满足门限的特征点阵的坐标值，计算出双目立体视觉 系统的测量坐标系和故障卫星之间的相对姿态、相对位置的初值，并将值发送给仿真控制 计算机。
[0128] 步骤45,关闭LED灯阵，完成确定故障航天器等效装置与双目立体视觉系统的相对 姿态的初值和相对位置的初值。
[0129] 利用方向余弦法确定特征点阵与视觉测量坐标系之间的姿态，则，
[0131] 其中，
为特征点阵与视觉测量坐标系之间的姿态：
,P1代表的是第一个特征点，P2代表的是第二个特征点，P3代表 的是第三个特征点。
[0132] 同时特征点阵与故障卫星等效装置的本体坐标系之间的姿态关系是已知的，该数 量关系是由机械加工保证的，该姿态记为则故障卫星等效装置相对于视觉测量坐标系 的姿态初值为：
[0134]然后计算故障卫星的等效质心在视觉测量坐标系中的位置，已经测得第二个特征 点P2在视觉测量坐标系中的坐标为X2 = ( X2，y2，Z2 )，同时已知等效质心相对于特征点阵的坐 标为y，则等效质心在视觉测量坐标系中的位置初值ZO为
[0136] 其中，%是及￡的转置矩阵，该等效质心在视觉测量坐标系中的位置初值ZQ即为 相对位置的初值。
[0137] 关闭LED灯阵，完成实验平台的标定。
[0138] 步骤5,仿真控制计算机利用步骤4中的得到的相对姿态初值、相对位置初值以及 仿真控制计算机生成的仿真场景通过相对姿态动力学模型和相对轨道动力学模型计算出 相对姿态和相对位置的姿态序列，然后将相应的控制指令分别发给三轴电动转台和三维平 移台，从物理上模拟出服务航天器与故障航天器之间的相对运动。
[0139] 根据步骤44中得到的等效质心在视觉测量坐标系中的位置初值ZO对两个航天器 之间的相对线运动描述为：
[0143] 其中，ω是航天器轨道角速率，z = (δχδγδζ)，是服务航天器相对于故障航天器在 轨道坐标系中的相对位置。该方程是微分方程，可以用数值积分算法计算预报出后续各个 时刻的线运动轨迹，预报初值就是等效质心在视觉测量坐标系中的位置初值ζο,然后将该 轨迹作为控制指令驱动三维平移台运动。
[0144] 根据步骤44中得到的故障卫星等效装置相对于视觉测量坐标系的姿态初值 对两个航天器之间的相对姿态运动描述为，
[0147] 其中，c〇ct是两个航天器之间的相对角速度，cocb表示是服务航天器的角速度，R (q。*)表示根据姿态四元数计算出姿态矩阵，Cotb表示是故障航天器的角速度，&*表示两个 航天器之间的相对姿态四元数。这两个方程也是微分方程，同样可以用数值积分算法实现 计算预报出后续各个时刻的角运动轨迹，预报初值就是故障卫星等效装置相对于视觉测量 坐标系的姿态初值计算出的。然后将角运动轨迹作为控制指令驱动三轴电动转 台运动。
[0148] 步骤6,开启同步触发电路，周期性地触发2台工业相机对故障航天器等效装置成 像。
[0149] 步骤7,开启导航计算机系统，周期性地对两个相机图像进行处理，然后利用待验 证的相对导航和辨识算法进行导航计算，将导航结果发给仿真控制计算机，仿真控制计算 机将导航结果与真值比较，作为评价待验证算法性能的依据。
[0150] 由上可知，平台的初始化时:利用棋盘格标定双目相机的内外参数，利用故障航天 器等效装置上一组LED灯作为已知特征点来辅助确定相对姿态和相对位置的初值。平台的 实验时:利用仿真控制计算机生成实验场景，控制三轴电动转台模拟服务航天器和故障航 天器之间的相对姿态运动，控制三维电动平移台来模拟服务航天器和故障航天器之间的相 对位置运动;脉冲电路周期触发双目立体视觉系统采集故障航天器等效装置的图像，导航 计算机处理双目图像，并利用待验证的相对导航算法模块进行导航与目标参数辨识计算， 将导航与参数辨识结果上传至仿真控制计算机，与仿真场景设置的真值进行比较和评估。 该半物理实验平台成本低，仿真逼真度高，能满足快速验证航天器在轨服务中相对导航算 法的需要。
[0151]上面结合附图所描述的本发明优选具体实施例仅用于说明本发明的实施方式，而 不是作为对前述发明目的和所附权利要求内容和范围的限制，凡是依据本发明的技术实质 对以上实施例所做的任何简单修改、等同变化与修饰，均仍属本发明技术和权利保护范畴。
【主权项】
1. 一种在轨服务相对导航实验平台，其特征在于:包括手动转台、棋盘格标定板、=轴 电动转台、=维平移台、仿真控制计算机、故障航天器等效装置、双目立体视觉系统与导航 计算机系统，其中： 所述棋盘格标定板设置于手动转台上，故障航天器等效装置安装在=轴电动转台上， 故障航天器等效装置表面安装有用于实验平台初次使用的标定特征点群，该特征点群由 LED灯构成； 双目立体视觉系统用于对棋盘格标定板的图像采集、LED灯阵的图像采集W及对故障 航天器等效装置的图像采集； 所述=轴电动转台模拟相对姿态运动，=维平移台模拟相对位置运动； 仿真控制计算机用于根据导航计算机系统发送相对姿态的初值和相对位置的初值生 成仿真场景并控制=轴电动转台和=维平移台运动；另外，仿真控制计算机还根据导航计 算机系统推送的导航参数与仿真场景中的仿真真值比较，作为评价待验证算法性能的依 据； 导航计算机系统根据采集的棋盘格标定板的图像对双目