随着技术的进步，人类的活动在不断地向太空扩展。据统计，全球平均每年发射80-130颗卫星，然而有2-3颗卫星未能正确入轨，而正确入轨的卫星中，又有5-10颗在寿命初期(入轨后前30天)即失效，其中，机械故障导致的卫星失效占了相当大的比例。典型的例子如我国的鑫诺二号卫星，于2006年10月29日发射升空后，卫星虽然成功定轨，且卫星的测控、姿轨控等分系统均处于良好的状态，然而，由于太阳帆板二次展开和天线展开未能完成，卫星无法正常工作，虽然科研人员采用多种方法进行营救，仍然未能恢复其功能，这颗耗资20亿人民币、设计寿命为15年的卫星随即成了一颗废星。对于那些完全失效或由于任务结束被放弃的卫星，停留在太空将成为太空垃圾，不但占用了宝贵的轨道资源，还可能危机其它航天器的安全。为了尽可能挽回损失或净化轨道环境，各国正在研究以空间机器人为手段，以卫星维修、生命延长及太空垃圾清除为目的的在轨服务技术。
为确保在轨任务的成功执行，需要在地面进行充分的仿真、实验以对空间机器人跟踪、接近、捕获目标卫星过程中的关键算法——视觉测量、路径规划、协调控制等进行验证和评估。对于空间机器人这样的系统，要同时开展微重力环境下两星交会、对接、捕获等过程的实验，目前还没有很好的解决方法(徐文福，梁斌，李成，等.空间机器人微重力模拟实验系统研究综述.机器人，2009，31(1)：88-96)。如常用的气浮式试验系统，仅能开展平面3个自由度——两个平动、一个转动的运动试验；自由落体或失重飞行试验系统的试验空间、试验时间非常有限，吊丝配重式试验系统的重力补偿精度低，且无法实现基座的自由漂浮；水浮式地面试验系统可以进行三维空间的微重力实验，实验时间也不受限制，但空间机器人原型样机不能直接在水浮系统上进行测试，必须进行专门的设计以免受到水下环境的影响，而且，水的阻力及惯量会改变空间机器人系统的动力学特性。另外，硬件在环内的半物理仿真实验系统，采用数学模型与实物相结合，通过动力学模型计算真实系统在三维空间的运动，然后通过实物来模拟其运动。由于运动范围有限、所能采用的实物有限，半物理仿真实验系统仅能进行部分任务的仿真实验。如文献(Wenfu Xu，Bin Liang，Yangsheng Xu，Cheng Li，Wenyi Qiang.A Ground Experiment System of Free-floating Space RobotFor Capturing Space Target[J].Journal of Intelligent and Robotic Systems.2007，48(2)：187-208)所建的地面实验系统，仅能进行目标捕获阶段的仿真验证，且未能对视觉测量的空间环境及光照条件进行模拟。数学仿真具有实现简单、投资少、灵活性高的优点，尤其在系统设计、任务分析、算法的理论验证等阶段，显得十分重要。然而，以往的仿真系统，往往仅考虑某一方面的仿真验证，而其他相关部分往往做简化处理，目前为止，还没有查阅到能同时开展图像处理与位姿测量、航天器GNC算法、空间机器人路径规划以及控制的动力学闭环仿真验证。因此，开发一套空间机器人视觉测量、路径规划、GNC一体化仿真系统是非常必要和迫切的。

本发明的目的在于克服现有技术的不足，提供了一种空间机器人视觉测量、路径规划、GNC一体化仿真系统。该系统不需要采用真实的测量设备、空间机器人系统及目标星，在一台独立的PC机上即可进行空间机器人跟踪、接近、捕获非合作目标过程中的所有关键算法的闭环仿真和验证。
本发明的空间机器人视觉测量、路径规划、GNC一体化仿真系统，包括：图像处理与位姿测量模块(1)、空间机器人规划与控制模块(2)、目标星控制器模块(3)、系统动力学模块(4)、系统3D几何模型模块(7)、双目相机模拟成像模块(6)及立体标定模块(5)。其中：
图像处理与位姿测量模块(1)，接收由双目相机模拟成像模块(6)输出的左、右相机图像Img_L[W][H]、Img_R[W][H]，并对图像进行实时处理，最后计算出非合作目标星相对于空间机器人基座和末端的相对位置和姿态该测量结果作为空间机器人规划与控制模块(2)的输入；
空间机器人规划与控制模块(2)，根据从图像处理与位姿测量模块(1)输入的相对位姿测量结果以及系统动力学模块(4)输入的系统当前状态——基座质心姿态Ψc、角速度ωc、机械臂关节角Θ、角速度自主规划跟踪接近过程中基座进行姿态、轨道控制的期望位姿轨迹，以及利用机械臂抓捕目标过程中机械臂关节角的运动轨迹，并调用相应的控制算法跟踪期望的轨迹，输出为系统质心控制力、基座姿态控制力矩以及机械臂各关节的控制力矩，该输出是系统动力学模块(4)的一部分输入；
目标星控制器模块(3)，建立目标卫星的控制器和执行机构模型，对目标进行位置、姿态的6DOF控制。该模块根据设定的目标运动模式，规划期望的位置、姿态轨迹，然后由该模块中的控制器、执行机构产生目标的控制力、力矩，实现目标按给定的规律运动，如三轴稳定、自旋稳定或翻滚卫星。该模块的输出为目标星动力学的作用力和力矩，是系统动力学模块(4)的另一部分输入；
系统动力学模块(4)，将两星的相对轨道动力学、空间机器人多刚体动力学以及目标星的姿态动力学集成在同一环境中，用于模拟整个空间机器人系统——包括基座及机械臂，及目标星的轨道、姿态、及多刚体运动状况。该模块的输入是空间机器人规划与控制模块(2)、目标星控制器模块(3)输出的各自由度(系统状态)的驱动力/力矩，输出为空间机器人系统质心位置rc、线速度vc、姿态Ψc、角速度ωc、机械臂关节角Θ、角速度以及目标星的质心位置rt、线速度vt、姿态Ψt、角速度ωt，作为系统几何模型模块(7)以及空间机器人规划与控制模块(2)的输入。
系统3D几何模型模块(7)，接收来源于系统动力学模块(4)输出的空间机器人系统质心位置rc、姿态Ψc、机械臂关节角Θ，以及目标星的质心位置rt、姿态Ψt，建立空间机器人系统、目标星的3D几何模型，以及航天器所在轨道环境的地球及星空背景模型，各模型由相应的状态变量实时驱动，显示各部分的3D运动情况；同时，该模块输出地球参考系、空间机器人系统本体坐标系以及目标卫星本体坐标系相对于左、右相机坐标系的位置、姿态，作为双目相机模拟成像模块(6)的输入；
双目相机模拟成像模块(6)，接收系统3D几何模型模块(7)输出的输出地球参考系、空间机器人系统本体坐标系以及目标卫星本体坐标系相对于左、右相机坐标系的位置、姿态，根据实际相机的光学参数——分辨率、视场角、畸变等，生成立体视觉系统的左、右相机各自所成的图像，并模拟图像采集过程，将叠加噪声后的图像信息存储在左、右相机图像矩阵Img_L[W][H]、Img_R[W][H]，其中W×H代表相机CCD的分辨率，典型的为512×512。该模块的输出即为Img_L[W][H]、Img_R[W][H]，作为图像处理与位姿测量模块(1)的输入。
立体标定模块(5)，包括左右相机标定图像生成、立体标定算法等子模块。标定图像可采用棋盘格或圆形光标，通过设定不同的位置、指向，提取21幅图，然后采用最小二乘法标定相机的参数。标定数据用于非合作目标识别算法中。
所述的图像处理与位姿测量模块(1)包括图像处理、3D重构及位姿测量三个子模块。图像处理子模块接收自双目相机模拟成像模块(6)输入的左、右相机图像，并采用合适的图像处理算法分别对左、右相机图像进行处理，提取出目标特征点在左、右相机图像中的2D图像信息；3D重构子模块利用已由立体标定模块(5)标定好的相机投影模型——左、右相机投影矩阵分别为CL和CR，对由图像处理子模块输出的特征点的2D图像信息进行立体匹配，得到特征点在世界坐标系中的3D坐标；位姿测量子模块根据特征点的3D坐标构造目标参考坐标系，并计算该参考坐标系相对于世界坐标系的位置和姿态
所述的图像处理算法包括图像滤波、边沿检测、直线提取。平滑滤波采用中值滤波算法，边沿检测采用Canny算法，直线提取采用Hough变换算法，中值滤波对于给定的n个数值{a1，a2，…，an}，将它们按大小顺序排列。当n为奇数时，位于中间位置的那个数值称为这n个数值的中值。当n为偶数时，位于中间位置的两个数值的平均值称为这n个数值的中值，记作med{a1，a2，…，an}，图像经过中值滤波后，某像素的输出等于该像素领域中的各像素灰度的中值；Canny边缘检测算法用高斯滤波器平滑图像、用一阶偏导的有限差分来计算梯度的幅值和方向、对梯度幅值应用非极大值抑制、用双阈值算法检测和连接边缘；Hough变换算法用点-线(点-直线或点-曲线)的对偶性，将图像空间x-y中的直线，变换成为参数空间的一点；
所述的空间机器人规划与控制模块(2)包括基座GNC、基座执行机构、空间机器人路径规划器、关节控制器四个子模块。基座GNC根据当前系统状态——基座姿态Ψc、角速度ωc、机械臂关节角Θ、角速度以及目标卫星的相对位姿测量结果执行空间机器人跟踪、接近、停靠目标卫星过程中的GNC算法，产生各执行机构的控制指令——反作用飞轮的控制电压、推力器的推力脉冲、磁力矩器的控制电压；基座执行机构包括反作用飞轮组件、推力器组件以及磁力矩器模型，将基座GNC子模块的控制指令，转换为基座质心作用力fb、力矩τb；空间机器人路径规划器根据当前系统状态和目标卫星的相对位姿测量结果，规划空间机器人捕获、维修目标卫星过程中机械臂各关节角的轨迹——期望关节角Θd、角速度关节控制器子模块包括六个关节的控制器，各控制器接收路径规划器规划的期望关节角Θd和角速度采用PID控制律，生成机械臂各关节的驱动力矩τb；
所述的GNC算法包括相对导航、制导律、姿态与轨道控制等部分。相对导航分别根据相对位置、姿态测量值，建立线性平移状态观测器、相对姿态状态观测器，将得到的平移位置矢量估计值、平移速度矢量估计值，作为平移控制的测量输入，而将得到的相对姿态估计值和相对角速度估计值，作为相对姿态控制的测量输入。制导律包括VBar直线接近制导律和位置保持制导律，在直线制导律下，空间机器人沿目标卫星轨道坐标系的-VBar方向进行直线接近，其余两个方向参考轨迹为零；相对位置保持制导律用于对停靠点的保持，使空间机器人和目标卫星维持在一个稳定的相对位置，在位置保持制导律下，设计的相对速度为零，标称相对位置为某一固定值。姿态与轨道控制包括基于飞轮的姿态控制、基于喷气的姿态控制，以及姿态与轨道六自由度控制，其中六自由度的控制通过姿态确定和相对导航的数据加上补偿信息，由控制器算法得出作用在星体上的力矩和外力信息，通过推力器的分配得出各个推进阀的工作状态。
所述的空间机器人路径规划器实现了一种空间机器人捕获非合作目标的自主路径规划方法，该方法利用手眼相机的相对位姿测量值，实时规划空间机器人的运动，以最终捕获目标。主要包括如下步骤：位姿偏差计算、目标运动的预测、空间机器人末端运动速度规划、空间机器人避奇异的路径规划、基座运动的预测等。首先，根据手眼测量数据判断相对位姿偏差ep和eo是否小于设定的阈值εp和εo，即捕获区域，三轴相对位置在10mm以内，三轴姿态偏差在1°左右。若小于，则闭合手爪、捕获目标；反之，则根据相对位姿偏差，实时估计目标的运动状态，并将估计的结果反应到机械臂末端速度的规划中，以保证机械臂末端时刻朝最近的方向(直线)趋近目标，机械臂末端能自主跟踪目标的运动，直到最后捕获目标。规划出末端运动速度后，即调用自主奇异回避算法，以解算关节的期望角速度，并据此预测机械臂运动对基座的扰动，当扰动超出容许的范围时，则自动调整机械臂的关节运动速度，以保证期望的偏转在许可的范围内。整个过程一直持续到机械臂捕获到目标为止。
所述的空间机器人视觉测量、路径规划、GNC一体化仿真系统，其特征在于所述的目标星控制器模块(3)设定的目标卫星姿态、轨道机动目标，采用6DOF控制规律，产生目标卫星质心位置控制力ft、姿态控制力矩τt；
所述的系统动力学模块(4)包括空间机器人系统的轨道动力学模型、多刚体动力学模型、目标卫星的姿态及轨道动力学模型。空间机器人系统的轨道动力学模型接收基座执行机构输出的喷气作用力，计算系统质心的位置、线速度；空间机器人系统多刚体动力学接收基座执行机构输出的姿态控制力矩、机械臂关节控制器输出的关节驱动力矩，计算由基座和机械臂组成的多刚体系统的状态变化——姿态Ψc、角速度ωc、机械臂关节角Θ、角速度目标卫星姿态、轨道动力学接收由目标星控制器模块(3)输出的作用力、作用力矩，计算目标卫星质心位置rt、线速度vt、姿态Ψt、角速度ωt；
所述的系统几何模型模块(7)包括轨道环境、空间机器人3D模型、目标星3D模型三个子模块，各子模块采用OpenGL进行建模。轨道环境模型子模块根据空间机器人及目标卫星所处的轨道位置，绘制地球背景、星空背景；空间机器人3D模型子模块根据基座质心位置rc、姿态Ψc、机械臂关节角Θ绘制空间机器人系统的3D几何模型；目标星3D模型子模块根据目标卫星质心位置rt、姿态Ψt，绘制目标卫星的3D几何模型。该模块还输出地球参考系、空间机器人系统本体坐标系以及目标卫星本体坐标系相对于左、右相机坐标系的位置、姿态；
所述的双目相机模拟成像模块(6)由左相机图像生成、左相机图像采集、右相机图像生成、右相机图像采集等子模块组成。相机图像生成子模块接收3D几何模型模块(7)输出的地球参考系、空间机器人系统本体坐标系以及目标卫星本体坐标系相对于左、右相机坐标系的位置、姿态，根据相机的分辨率、视场角、畸变系数等，采用透视投影模型，生成相应相机所观察到的图像；图像采集子模块模拟图像采集过程中的噪声传递过程，将噪声数据叠加于所生成的图像，图像数据存储在矩阵Img_L及Img_R中，作为图像处理与位姿测量模块(1)的输入；
所述的空间机器人视觉测量、路径规划、GNC一体化仿真系统，其特征在于所述的图像噪声包括热噪声、电子噪声、放大器噪声以及量化噪声，采用加性高斯白噪声进行模拟。
所述的空间机器人视觉测量、路径规划、GNC一体化仿真系统，其特征在于所述的立体标定模块(5)包括左相机标定图像生成、右相机标定图像生成、以及立体标定算法三个子模块。左、右相机标定图像生成子模块分别生成位于世界坐标系下不同位置处的标定版在左、右相机中的图像，标定版为7×7圆形光标阵列，共生成标定图像21幅；立体标定算法根据所生成的21幅标定图像，采用最小二乘法分别对左、右相机投影模型进行标定，得出左、右相机投影矩阵CL和CR。
本发明与现有技术相比具有如下优点：(1)系统成本低、安全性、灵活性好，且易于实现。不需要采用真实的测量设备、空间机器人系统及目标星，在一台独立的PC机上即可进行空间机器人跟踪、接近、捕获非合作目标过程中的所有关键算法的闭环仿真和验证；(2)将多刚体动力学、轨道、姿态动力学集成一体，开展18DOF的联合仿真，更真实、准确的反映系统的动力学特性；(3)模拟的相机图像考虑了航天器飞行轨道上的地球背景、星空背景、及图像采集噪声，更准确地反映了在轨视觉测量的恶劣条件；(4)采用的非合作目标自主识别与位姿测量，不要求在目标星上安装用于辅助测量的发光标志器或角反射镜，也无需知道目标的几何尺寸，直接识别航天器自身的部件，并计算相对位姿；(5)采用的空间机器人目标捕获自主路径规划方法，能实时预测目标的运动状态、自主回避运动学及动力学奇异、实时预测机械臂运动对基座产生的干扰并自主调整规划的路径；(6)采用的协调控制方法，既考虑了整个空间机器人系统的控制性能，又适应了当前的星载处理器的控制能力。
附图说明
图1是典型的空间机器人在轨服务流程；
图2是空间机器人系统及非合作目标在轨捕获示意图；
图3是空间机器人视觉测量、路径规划、GNC一体化仿真系统组成图；
图4是空间机器人视觉测量、路径规划、GNC一体化仿真系统的界面；
图5是空间机器人视觉测量、路径规划、GNC一体化仿真系统的立体标定界面；
图6是双目立体视觉测量原理；
图7是非合作目标识别与位姿测量算法流程；
图8是所识别出的非合作目标的帆板支架；
图9是交会参考坐标系与捕获点坐标系的定义；
图10是飞轮姿态控制框图；
图11是喷气姿态控制框图；
图12是喷气轨道控制框图；
图13是姿态与轨道六自由度喷气控制框图；
图14是空间机器人目标捕获自主路径规划流程图；
图15是推力器的布局图；
图16是单臂空间机器人系统运动学链示意图；
图17是OpenGL的堆栈原理；
图18是三维物体模型类的继承关系；
图19是3D几何建模的流程图；
图20是基于透视投影的虚拟图像生成原理；
图21是模拟生成的虚拟左相机图像(512×512黑白)；
图22是模拟生成的虚拟右相机图像(512×512黑白)；

一、系统组成
典型的空间机器人在轨服务思想如图1所示，即利用空间机器人系统，对故障卫星进行跟踪、接近、捕获、对接、维修等操作。在轨服务流程大致可分为如下几个阶段：
(i)远距离跟踪接近：空间机器人系统从自身轨道机动到距离目标卫星300m左右的位置；
(ii)中距离跟踪接近：空间机器人系统从300m机动到距离目标卫星15m左右的位置；
(iii)近距离交会与停靠：空间机器人系统从15m左右的位置机动到1m左右的位置，并进行相对停靠，使得目标卫星处于机械臂的工作空间内；
(iv)在轨抓捕与对接：利用空间机械手抓捕目标卫星，并将其与空间机器人基座对接在一起；
(v)在轨维修或执行离轨操作，即将目标带入地球大气层或坟墓轨道：利用机械臂对目标卫星进行在轨维修，或者将目标带入地球大气层或坟墓轨道。
假设待服务对象为一帆板未展开的故障卫星，其上未预先安装用于视觉测量的标志器和用于捕获的手柄，即为非合作目标。所设计的空间机器人在轨服务系统由一飞行基座和机械手组成，如图2所示。其中，飞行基座上安装了目标测量系统——基座立体视觉、对接机构、姿轨控系统等，空间机械手由6DOF机械臂、抓捕手爪及末端立体视觉组成。将利用空间机器人系统，对故障卫星进行跟踪、接近、抓捕、维修等操作。
为对整个空间机器人在轨服务过程进行闭环仿真，开发了一套空间机器人视觉测量、路径规划、GNC一体化仿真系统，其组成如图3所示，包括六个大的模块：图像处理与位姿测量模块、空间机器人规划与控制、目标控制器、系统动力学模型、系统几何模型及双目相机模型。“图像处理与位姿测量模块”主要是处理双目立体相机的图像——包括图像滤波、边沿检测及直线提取等，并提供目标的相对位姿测量值。“空间机器人规划与控制”根据相对位姿测量数据，规划基座的期望位姿和机械臂关节角的运动轨迹，并调用相应的控制算法跟踪期望的轨迹。“目标星控制器”用于产生目标的控制力矩，以实现目标按给定的规律运动，如三轴稳定、自旋稳定或翻滚卫星。“系统动力学模型”包括相对轨道动力学、空间机器人动力学及目标姿态动力学模型，用于模拟真实对象的运动，其输入为各自由度的驱动力/力矩，输出为各自由度的状态变化。“系统几何模型”的功能是根据各部分——空间机械臂、基座、目标星、地球及星空背景的几何参数和当前状态绘制并显示各部分的3D运动情况。系统的3D模型通过OpenGL建立。“双目相机模型”根据实际相机的光学参数——分辨率、视场角，以及安装位置，产生实际相机所成的像。
所开发的非合作目标自主识别与捕获全数学仿真系统如图4所示，该系统还可进行双目立体视觉标定，如图5所示。
二、非合作目标自主识别与位姿测量算法
(1)相机成像及3D重构模型
针孔模型(pin-hole model)是最常用的相机成像模型，可表示为
$\lambda \left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=C\left[\begin{array}{c}{X}_W\\ Y_W\\ Z_W\\ 1\end{array}\right]---\left(1\right)$
其中，WP＝[XW，YW，ZW]T为空间某点P在世界坐标系中的坐标，(u，v)是以像素为单位的图像坐标，C为相机的投影变换矩阵，由其内外参数确定。基于立体视觉的3D重建原理如图6所示，假定第i个特征点Pi在世界坐标系中的位置为WPi＝[XWi YWi ZWi]T，投影在左右相机的图像坐标分别为pLi＝[uLi vLi]T和pRi＝[uRi vRi]T。根据(1)，对于左、右相机，有如下关系：
$\left[\begin{array}{c}{\lambda }_L{u}_{L_i}\\ {\lambda }_L{v}_{\mathrm{Li}}\\ {\lambda }_L\end{array}\right]=C_L\left[\begin{array}{c}{X}_{\mathrm{wi}}\\ Y_{\mathrm{wi}}\\ Z_{\mathrm{wi}}\\ 1\end{array}\right]=\left[\begin{array}{cccc}1& {\alpha }_1& {\alpha }_2& {\alpha }_3\\ {\alpha }_4& {\alpha }_5& {\alpha }_6& {\alpha }_7\\ {\alpha }_8& {\alpha }_9& {\alpha }_{10}& {\alpha }_{11}\end{array}\right]\left[\begin{array}{c}{X}_{\mathrm{wi}}\\ Y_{\mathrm{wi}}\\ Z_{\mathrm{wi}}\\ 1\end{array}\right]---\left(2\right)$
$\left[\begin{array}{c}{\lambda }_R{u}_{\mathrm{Ri}}\\ {\lambda }_R{v}_{\mathrm{Ri}}\\ {\lambda }_R\end{array}\right]=C_R\left[\begin{array}{c}{X}_{\mathrm{wi}}\\ Y_{\mathrm{wi}}\\ Z_{\mathrm{wi}}\\ 1\end{array}\right]=\left[\begin{array}{cccc}1& {\beta }_1& {\beta }_2& {\beta }_3\\ {\beta }_4& {\beta }_5& {\beta }_6& {\beta }_7\\ {\beta }_8& {\beta }_9& {\beta }_{10}& {\beta }_{11}\end{array}\right]\left[\begin{array}{c}{X}_{\mathrm{wi}}\\ Y_{\mathrm{wi}}\\ Z_{\mathrm{wi}}\\ 1\end{array}\right]---\left(3\right)$
矩阵CL、CR分别为左右相机的变换矩阵，αi、βi为对应的元素。对式(2)和(3)进行化简，有：
$\left[\begin{array}{ccc}1-{\alpha }_8{u}_{\mathrm{Li}}& {\alpha }_1-{\alpha }_9{u}_{\mathrm{Li}}& {\alpha }_2-{\alpha }_{10}{u}_{\mathrm{Li}}\\ {\alpha }_4-{\alpha }_8{v}_{\mathrm{Li}}& {\alpha }_5-{\alpha }_9{v}_{\mathrm{Li}}& {\alpha }_6-{\alpha }_{10}{v}_{\mathrm{Li}}\\ 1-{\beta }_8{u}_{\mathrm{Ri}}& {\beta }_1-{\beta }_9{u}_{\mathrm{Ri}}& {\beta }_2-{\beta }_{10}{u}_{\mathrm{Ri}}\\ {\beta }_4-{\beta }_8{v}_{\mathrm{Ri}}& {\beta }_5-{\beta }_9{v}_{\mathrm{Ri}}& {\beta }_6-{\beta }_{10}{v}_{\mathrm{Ri}}\end{array}\right]\left[\begin{array}{c}{X}_{\mathrm{wi}}\\ Y_{\mathrm{wi}}\\ Z_{\mathrm{wi}}\end{array}\right]=\left[\begin{array}{c}{\alpha }_{11}{u}_{\mathrm{Li}}-{\alpha }_3\\ {\alpha }_{11}{v}_{\mathrm{Li}}-{\alpha }_7\\ {\beta }_{11}{u}_{\mathrm{Ri}}-{\beta }_3\\ {\beta }_{11}{v}_{\mathrm{Ri}}-{\beta }_7\end{array}\right]---\left(4\right)$
写成矩阵的形式：
K·WPi＝U               (5)
式(5)有四个方程、三个未知数，为超越方程，可用最小二乘法进行求解：
WPi＝(KTK)-1KTU         (6)
上述过程即为基于立体视觉的3D重构过程。
(2)基于立体视觉的相对位姿测量算法
基于立体视觉的相对位姿测量算法流程如图7所示。主要包括如下步骤：
(i)图像滤波：分别对左右相机图像进行滤波，以消除噪声干扰，得到平滑的左右相机图像实际获得的图像一般都含有噪声，本发明采用中值滤波算法进行图像平滑。中值滤波是一种抑制噪声的非线性处理方法。对于给定的n个数值{a1，a2，…，an}，将它们按大小顺序排列。当n为奇数时，位于中间位置的那个数值称为这n个数值的中值。当n为偶数时，位于中间位置的两个数值的平均值称为这n个数值的中值，记作med{a1，a2，…，an}。图像经过中值滤波后，某像素的输出等于该像素领域中的各像素灰度的中值。中值滤波的方法运算简单，易于实现，而且能较好地保护边界。
(ii)边沿检测：对两幅图像分别进行边沿检测，得到边沿特征信息
边缘(edge)是指图像局部强度变化最显著的部分。边缘主要存在于目标与目标、目标与背景、区域与区域(包括不同色彩)之间，是图像分割、纹理特征和形状特征等图像分析的重要基础。本文采用Canny算法对图像进行边缘检测。Canny边缘检测算法的步骤如下：(a)用高斯滤波器平滑图像；(b)用一阶偏导的有限差分来计算梯度的幅值和方向；(c)对梯度幅值应用非极大值抑制；(d)用双阈值算法检测和连接边缘。
(iii)直线提取：对进行边沿检测后的图像进行直线提取，得到包含三角形支架各直线在内的各条直线信息
在利用Canny算法检测出图像边沿后，再采用Hough变换提取直线。哈夫变换的基本思想是利用点-线(点-直线或点-曲线)的对偶性，即图像空间x-y中的直线，经过Hough变换后成为参数空间的一点。
Hough变换算法的步骤如下：(a)适当地量化参数空间；(b)假定参数空间的每一个单元都是一个累加器，把累加器初始化为零；(c)对图像空间的每一点，在其所满足的参数方程对应的累加器上加1；(d)累加器阵列的最大值对应模型的参数。
(iv)三角支架的识别：从提取后的所有直线信息中，识别对应于三角形帆板支架的六条直线
采用Hough变换后，可以提取出多条直线，包括目标卫星的轮廓、太阳电池阵列及太阳帆板支架。由于帆板支架有明显的特征，故以帆板支架为识别对象。从众多直线中识别出属于帆板支架的六条直线，是本文研究的关键。由于除了已知帆板支架是一三角形外，没有其他任何先验知识，包括三角形的边长、内角、各边的倾斜度等，因此，识别的难度可想而知。在此假定图像可通过遥测通道下传到地面，地面操作员根据该图像设置两个初始参考点pr1、pr2，星载程序即可完成对帆板支架的自动识别，随即转入跟踪模式，以后能够自动根据图像，实时跟踪帆板支架所在的区域。参考点pr1为三角形内的任意一点，pr2为三角形外且位于太阳电池阵列上的任意一点，pr1、pr2通过鼠标直接在遥测图像中选取，没有特别严格的要求。识别的原理及结果如图8所示。
(v)交点特征的提取：利用识别出来的三角形支架对应的六条直线，计算出三角形的顶点
(vi)立体匹配、3D重构：根据识别出的三角形支架各顶点分别在左、右相机中的2D信息，进行各特征点的3D重构，得到三角形支架各顶点在世界坐标系中的3D坐标，即WP1～WP6
(vii)目标位置、姿态的测量：根据3D重构的结果，进一步构建抓捕对象坐标系，并计算出其相对于世界坐标系——追踪星测量坐标系的位置和姿态。
根据已识别出的六个特征点，可建立目标的“交会参考坐标系”∑R2和“捕获点坐标系”∑Cap，如图9所示。首先，支架中心的三个点(Q1～Q3)的三维坐标由下面几式确定：
${}^{W}Q_1=\frac{{}^{W}P_1+{}^{W}P_2}{2}---\left(7\right)$
${}^{W}Q_2=\frac{{}^{W}P_3+{}^{W}P_4}{2}---\left(8\right)$
${}^{W}Q_3=\frac{{}^{W}P_5+{}^{W}P_6}{2}---\left(9\right)$
则中点
${}^{W}M=\frac{{}^{W}Q_1+{}^{W}Q_2}{2}---\left(10\right)$
坐标系∑R2的原点为
$r_{R2}={}^{W}O_t=\frac{{}^{W}Q_3+{}^{W}M}{2}---\left(11\right)$
坐标系∑R2的X轴为单位矢量：
$n_{R2}=\frac{Q_3{Q}_1\times Q_3{Q}_2}{\left|\right|Q_3{Q}_1\times Q_3{Q}_2\left|\right|}---\left(12\right)$
坐标系∑R2的Y轴为单位矢量：
$o_{R2}=\frac{Q_{3}M}{\left|\right|Q_{3}M\left|\right|}---\left(13\right)$
坐标系∑R2的Z轴由右手定则确定：
aR2＝nR2×oR2                  (14)
则坐标系∑R2相对于交会参考系∑R1的姿态旋转矩阵为
R1AR2＝[nR2 oR2 aR2]           (15)
姿态欧拉角可通过姿态旋转矩阵R1AR2求出，结合式(11)求出的相对位置，至此，目标的相对位置和姿态均已测出。类似的，“捕获点坐标系”∑Cap相对于“交会参考系”∑R1的位置、姿态亦可计算出。
三、空间机器人路径规划与控制
(1)基座GNC
相对导航：由于基座立体相机只能提供相对位置和相对姿态角信息，为得到相对速度和相对姿态角速度信息并降低复杂性，采用状态观测器方案对上述信息进行估值。由交会相机提供出的相对位置矢量测量值，建立一个线性平移状态观测器，将观测器得到的平移位置矢量估计值和平移速度矢量估计值，作为平移控制的测量输入。为减少状态观测器的延迟带来的影响，采用推力器管理功能模块提供预测控制力信息；根据相机给出的相对姿态角测量值，建立一个线性相对姿态状态观测器，由该线性观测器得到的相对姿态估计值和相对角速度估计值，作为相对姿态控制的测量输入。同样，为减少状态观测器的延迟带来的影响，采用推力器管理功能模块或飞轮控制模块部分提供预测控制力矩信息。
制导律设计：(i)VBar直线接近与后撤制导律，适用于在中、近距离的接近与后撤操作。在该制导律下，空间机器人沿目标卫星轨道坐标系的-VBar方向进行直线接近与后撤，其余两个方向参考轨迹为零。分别设置最大加速度限制，根据初速度、加速度、时间、距离等要求，计算二元方程，得出和时间相关的参考轨迹和速度；(ii)位置保持制导律，适用于各距离段的相对停靠点保持，使空间机器人和目标卫星维持在一个稳定的相对位置。在该制导律下，设计的相对速度为零，标称相对位置为某一固定值，相对位置设置一个不控区间，在此区间内不进行位置控制，否则以标称相对位置为目标进行控制。区间的选择根据位置控制精度指标来选择，在近距离停靠保持期间，不控区为零或较小，距离远处设置较大；(iii)远距离接近与后撤的C-W制导律，适用于空间机器人相对目标卫星的远距离接近与后撤操作，在此制导律下，空间机器人根据相对运动的末端期望位置和速度，在初始时间点根据C-W方程计算需要的速度增量和末端需要的速度增量，并在某些固定的时间点计算当前速度偏差来决定是否中途修正。C-W制导律的计算结果与当前位置和速度、末端位置和速度以及飞行时间有关。
控制算法设计：空间机器人的控制包括姿态控制和轨道控制两类，姿态控制包括非任务期间的速率阻尼、地球捕获和定向、对日捕获和定向、变轨时姿态稳定控制、全姿态捕获控制，以及任务期间的对目标定向和对地定向控制，速率阻尼、变轨以及连续轨道控制时采用喷气控制模式，其他模式主要采用零动量反作用飞轮进行控制。轨道控制包括相对位置保持、跟踪接近与后撤以及伴随飞行等多种，采用喷气控制来实现。对于姿态和轨道均采用喷气控制的情况，设计六自由度控制策略实现姿态和轨道的同步控制。
(i)基于飞轮姿态控制
当采用飞轮控制时，采用姿态四元数作为姿态参数。为满足系统稳态精度与动态性能要求，轮控设计基于以下原则：减少帆板挠性振动的影响；克服系统测量噪声及飞轮摩擦力矩的影响；克服外干扰的影响；考虑惯量时变，保证系统有一定的稳定裕度。采用PID控制器，结合补偿器对星体进行控制，控制框图如图10。飞轮卸载采用磁卸载和喷气卸载两种，磁卸载公式为：
${\overrightarrow{T}}_u=\overrightarrow{M}\times \overrightarrow{B}=-\left(\frac{K}{B^2}\right)(\overrightarrow{B}\times \Delta {\overrightarrow{H}}_e)\times \overrightarrow{B}---\left(16\right)$
当整星某轴角动量超过启控阈值时，启动磁卸载，当角动量低于给定的脱控阈值时，断开磁卸载控制。喷气卸载在如下情况时工作：当外扰力矩作用使轮子达到饱和时，启动喷气卸载。此时姿态控制转入运行段喷气控制模式，反作用飞轮按脱控阈值保持状态模式进行工作。当飞轮的转速达到脱控阈值时自动重新进入轮控模式。
(ii)基于喷气的姿态控制方法
采用PD控制器，结合补偿器对星体进行控制。推力器采用脉宽调制方式输出。控制框图如图11所示。
(iii)喷气轨道控制设计
采用PD控制器，结合补偿器对星体进行控制。推力器采用脉宽调制方式输出。控制框图如图12所示。
(iv)姿态与轨道六自由度喷气控制
若姿态无法做到全力偶控制，这样，由喷气实现的姿态控制必然会导致轨道的变化，对于空间交会的两航天器来说，在姿态控制的同时考虑轨道的影响是必须的。六自由度控制的基本思想是通过姿态确定和相对导航的数据加上补偿信息，由控制器算法得出作用在星体上的力矩和外力信息，通过推力器的分配得出各个推进阀的工作状态。控制框图如图13所示。
(2)机械臂捕获目标的自主路径规划
上述图像处理与位姿测量算法给出了“捕获点坐标系”∑Cap相对于“交会参考系”∑R1的位置、姿态，自主规划算法首先将该测量结果先转化为“捕获点坐标系”∑Cap相对于“机械臂末端坐标系”的位置、姿态，然后根据此结果，实时规划空间机器人的运动，以最终捕获目标。主要包括如下步骤：手眼相机测量、位姿偏差计算、目标运动的预测、空间机器人末端运动速度规划、空间机器人避奇异的路径规划、基座运动的预测等。首先，根据手眼测量数据判断相对位姿偏差ep和eo是否小于设定的阈值εp和εo，若小于，则闭合手爪、捕获目标；反之，则根据相对位姿偏差，实时估计目标的运动状态，并将估计的结果反应到机械臂末端速度的规划中，以保证机械臂末端时刻朝最近的方向趋近目标，机械臂末端能自主跟踪目标的运动，直到最后捕获目标。规划出末端运动速度后，即调用自主奇异回避算法，以解算关节的期望角速度，并据此预测机械臂运动对基座的扰动，当扰动超出容许的范围时，则自动调整机械臂的关节运动速度，以保证期望的偏转在许可的范围内。整个过程一直持续到机械臂捕获到目标为止，其流程如图14所示。
(3)基座执行机构
采用反作用“三正交+一斜装”的反作用飞轮、“20+1推力器”以及磁力矩器作用执行机构。
(i)反作用飞轮
采用4个反作用飞轮，安装方式为3正交加1斜装。其中三个飞轮的输入轴沿星体的X、Y、Z轴正交安装，第四个斜装飞轮作为备份，与本体+X、-Y、-Z轴成54.73°安装。
在力矩模式中，飞轮的输入是力矩指令。通过调节电机的电枢电流I产生所需的电磁力矩，克服了轴承摩擦力矩之后，得到加速飞轮的力矩，施于卫星的控制力矩是飞轮的反作用力矩。
单个飞轮考虑动摩擦力矩、动不平衡力矩、粘性摩擦力矩，飞轮组考虑飞轮组与星体及飞轮组自身的耦合干扰力矩，其计算模型如下：
飞轮动力学模型：
${\stackrel{·}{H}}_w=T_d---\left(17\right)$
ωw＝Hw/Jw                    (18)
式中
Jw——飞轮的转动部分的转动惯性量；
Hw——飞轮的角动量；
ωw——飞轮的角速率；
Td——飞轮轴的驱动合力矩。
飞轮轴的驱动合力矩的计算公式为：
Td＝KMVUw-Tf                  (19)
式中
KMV——飞轮的力矩电压系数；
Uw——飞轮的控制电压；
Tf——轮的摩擦力矩。
飞轮的摩擦力矩的计算公式为：
$T_f={\alpha }_w{\omega }_w\sqrt{\left|{\omega }_w\right|}+C_w{\omega }_w+\mathrm{sign}\left({\omega }_w\right)T_e---\left(20\right)$
式中
αw——气动阻力力矩系数；
Cw——飞轮的粘性摩擦力矩系数；
Te——飞轮的动摩擦力矩；
sign——符号函数。
(ii)推力器
为满足GNC的控制要求，推进分系统共采用20台1N推力器做为平移姿态控制推力器，分为每组10台的两组互为备份，分别装于质心平面的四个三机机组和底部的四个双机机组上，1台5N推力器做为轨控推力器，布置于底部平面的几何中心。推力器总体布局见图15。
推力器的建模采用脉宽调制的方式，即根据推力器控制指令——喷气时间，决定每个推力器的作用脉宽。
(iii)磁力矩器
采用3个磁力矩器，主要用于飞轮卸载。沿星体的X、Y、Z轴正交安装。磁力矩器的建模如下：
已知期望生成的磁矩为U_mt[3]，本体坐标系相对于地心-惯性坐标系的姿态四元素为Qbi，地磁场强度在惯性坐标系下的矢量为Bi，则其在本体坐标系下的矢量Bb为：
Bb＝Cbi×Bb                         (21)
Cbi为根据姿态四元素为Qbi计算得到的姿态变换矩阵。则磁力矩器产生的控制力矩为：
Tmag＝(Umt+Nnoierr)×Bb             (22)
其中Nnoierr为其随机误差。
(4)机械臂关节角控制器
当期望的机械臂关节角、角速度规划出来后，需要设计相应的控制策略，控制机械臂关节角跟踪所规划的轨迹。采用如下PD控制律进行控制：
${\tau }_m=k_p({\Theta }_d-\Theta )+K_d({\stackrel{·}{\Theta }}_d-\stackrel{·}{\Theta })---\left(23\right)$
上式中，Kp＝diag[kp1，kp2，…，kp6]，Kd＝diag[kd1，kd2，…，kd6]分别为各个关节的比例微分控制参数，为期望的关节角、角速度，为当前的关节角、角速度。
四、空间机器人系统及目标星的动力学建模
(1)航天器轨道动力学模型
记质点在惯性系下的位置为r1，在中心天体体固系下的惯性加速度aE，惯性系到中心天体体固系的坐标旋转矩阵为CEI，反之为CIE，则有
${\stackrel{··}{r}}_I=C_{IE}{a}_E\left(C_{\mathrm{EI}}{r}_I\right)---\left(24\right)$
上式中，求导为在惯性系下的导数，括号表示aE为CEIr1的函数。将上式积分即可得到质点在惯性系下的位置速度。
追踪星和目标星的轨道坐标系分别记为Oo1Xo1Yo1Zo1、Oo2Xo2Uo2Zo2，两航天器的相对位置(追踪星质心Oo1在目标星轨道坐标系Oo2Xo2Yo2Zo2中的坐标)rc＝[x，y，z]，相对速度为${\stackrel{·}{r}}_c=[\stackrel{·}{x},\stackrel{·}{y},\stackrel{·}{z}].$在两航天器相距较近，且均运行在近圆轨道的条件下，相对运动可用Hill方程进行简化描述：
$\left\{\begin{array}{c}\stackrel{··}{x}+2\omega \stackrel{·}{y}=f_x/m_1\\ \stackrel{··}{y}-2\omega \stackrel{·}{x}-3{\omega }^{2}y=f_y/m_1\\ \stackrel{··}{z}+{\omega }^{2}z=f_z/m_1\end{array}\right.---\left(25\right)$
其中，(fx，fy，fz)为施加在追踪星上的控制力在目标星轨道坐标系下的投影。m1为追踪星的质量，ω为轨道角速度，对圆轨道来说，ω为常值。
(2)航天器的姿态动力学模型
根据角动量定律，卫星绕其质心转动的角动量方程为：
$I_b{\stackrel{·}{\omega }}_b+{\omega }_b\times I_b{\omega }_b=T_c+T_d---\left(26\right)$
式中
Ib——星体转动惯量矩阵，在本体系下的表示，常数；
ωb——星体绝对角速度向量，在本体系下的表示；
Tc——作用于星体上的控制力矩，在本体系下的表示；
Td——作用于星体上的扰动力矩，在本体系下的表示。
在航天器姿态动力学建模中，需要考虑的扰动力矩包括：气动力矩、太阳辐射力矩、剩磁力矩和重力梯度力矩。
(3)空间机器人系统的多刚体动力学模型
空间机器人动力学建模和控制方面的重要研究成果参见文献(Xu Y，Kanade T.Space Robotics：Dynamics and Control[M].Kluwer Academic Publishers，November，ISBN 0-7923-9265-5，1992)。单臂空间机器人的模型如图16所示，由6自由度机械臂和作为基座的航天器组成。
空间机器人的一般运动学方程可表示为
$\left[\begin{array}{c}{v}_e\\ {\omega }_e\end{array}\right]=J_b\left[\begin{array}{c}{v}_0\\ {\omega }_0\end{array}\right]+J_m\stackrel{·}{\Theta }---\left(27\right)$
其中，ve、ωe为机械臂末端线速度和角速度；v0、ω0为基座的线速度(质心处)和角速度。Jb为与基座运动相关的雅可比矩阵，而Jm为与机械臂运动相关的雅可比矩阵。
对于自由漂浮模式——基座位置、姿态均不受控，系统的线动量和角动量守恒，将守恒方程代入化简，得到自由漂浮模式下空间机器人系统的运动学方程：
$\left[\begin{array}{c}{v}_e\\ {\omega }_e\end{array}\right]=J_g({\Psi }_0,\Theta ,m_i,I_i)\stackrel{·}{\Theta }---\left(28\right)$
其中Jg为空间机器人的广义雅可比矩阵，它是基座姿态、机械臂关节角，以及各刚体质量、惯量的函数。
空间机器人系统的动力学方程(拉格朗日动力学方程)为
$\left[\begin{array}{cc}{H}_b& H_{\mathrm{bm}}\\ H_{\mathrm{bm}}^T& H_m\end{array}\right]\left[\begin{array}{c}{\stackrel{··}{x}}_b\\ \stackrel{··}{\Theta }\end{array}\right]+\left[\begin{array}{c}{c}_b\\ c_m\end{array}\right]=\left[\begin{array}{c}{f}_b\\ {\tau }_m\end{array}\right]---\left(29\right)$
上式中
${\stackrel{··}{x}}_b=\left[\begin{array}{c}{\stackrel{·}{v}}_0\\ {\stackrel{·}{\omega }}_0\end{array}\right]---\left(30\right)$
cb、cm∈R6分别为与基座运动和机械臂运动相关的非线性力，包括向心力和哥氏力；Fb∈R6为作用于基座的力和力矩，τm∈R6为机械臂关节的驱动力矩。
(4)动力学模型的VC实现
上述的动力学计算公式(25)、(26)及(29)均可写成改写成如下形式：
$\stackrel{··}{X}=f(X,\stackrel{·}{X},U)---\left(32\right)$
其中，X为状态变量，U为驱动力、力矩，为相应的方程。以相对轨道动力学方程(25)为例
X＝[x，y，z]T       (32)
U＝[fx，fy，fz]T    (33)
$f(X,\stackrel{·}{X},U)=\left[\begin{array}{c}{f}_x/m_s-2\omega \stackrel{·}{y}\\ f_y/m_s+2\omega \stackrel{·}{x}\\ f_z/m_s-{\omega }^{2}z\end{array},+3{\omega }^{2}y\right]---\left(34\right)$
采用四阶Runge-Kutta法对方程(31)进行数值积分，即可计算下一个时刻两星的相对位置和速度。
五、3D几何模型
利用OpenGL(Open Graphics Libaray)实现三维建模和实时仿真。OpenGL，是一个3D图形库，它被定义为“一个图形硬件的软件接口”，是计算机工业标准应用程序接口，主要用于定义二维和三维图形。OpenGL是用于开发简捷的交互式二维和三维图形应用程序的最佳工具，任何高性能的图形应用程序，从3D动画、CAD辅助设计到可视化仿真，都可以利用OpenGL高质量、高性能的特点，因此成为专业图形处理、科学计算等高端应用领域的标准图形库。在实际编程中，为了保存几何变换和投影变换的操作，OpenGL维护两个栈，即投影变换栈和几何变换栈。栈中保存的元素即为投影变换和几何变换的变换矩阵。在调用几何变换操作时，OpenGL将该几何变换操作的变换矩阵与当前栈的栈顶元素相乘，得到一个新的矩阵并将其作为几何变换栈的栈顶。两个栈之间的关系如图17所示，压栈操作的同时会把数据复制到新的栈顶。
由于机械臂各臂杆之间的关系建立在其D-H参数的基础上，三维建模时也按照这一原理，因此在绘制每个杆臂的模型前要先在前一个杆臂坐标系的基础上建立当前杆臂坐标系，然后才在新建立的坐标系下绘制杆臂，以此类推一直到机械臂末端的执行器。但是，绘制好执行器的模型后，当前的坐标系就留在了机械臂的末端执行器上；通过出栈操作就能实现几何变换矩阵的快速恢复，使其对应于初始的轨道坐标系，在此坐标系下进行目标卫星的几何建模。建模时，采用面向对象的编程思想，建立一个基本的物体模型类CBase3dObj，该类有6个位姿属性——成员变量，三轴平移和三轴旋转、设置颜色以及建模等方法。并在这个基本类的基础上，派生出虚拟仿真环境中所有物体对应的类，如图18所示。这些物体包括：空间机器人基座CChaser、机械臂CArm、目标星CTarget和太空场景CScene。其中，空间机器人、目标星、太空场景与轨道坐标之间的关系由位姿属性确定，太空场景主要指星星和地球。每个类中都包含了对应物体的几何尺寸、颜色等属性，并在其主建模函数中，包含了一系列基本的几何体坐标变换和绘制函数，在太空场景类中还要实现纹理图像的读取和映射。
在这些基本物体模型类的基础上，我们就能建立整个虚拟仿真环境，具体过程如下：初始化完成后，先设置好虚拟相机坐标系相对于轨道坐标系的位姿，并保留当前的坐标系关系——几何变换阵入栈；接着设置空间机器人基座坐标系相对于轨道坐标系的位姿，然后调用母星模型的绘制函数；在此当前坐标转移的基础上，根据机械臂的安装位置，调用机械臂类对象的坐标平移和旋转函数实现坐标系的变换，之后就能在母星上绘出机械臂的模型；绘制好母星及机械臂后，恢复当前坐标系到轨道坐标系，然后根据目标星的漂浮轨迹设置好目标星坐标系相对于轨道坐标系的位姿，再调用目标星模型的绘制函数。总体软件实现流程如图19所示。
六、双目相机成像模型
通过在OpenGL中，设置透视投影来产生模拟的相机图像，透视投影函数为：
void gluPerspective(GLdouble fovy，GLdouble aspect，GLdouble zNear，GLdouble zFar)；
上述函数创建一个对称透视视景体，参数fovy为视野在X-Z平面的角度，aspect是投影平面宽度与高度的比率；zNear和zFar分别是近、远裁剪面到视点的距离。透视投影的含义如图20所示。根据相机的视场角FOV、安装位置及分辨率Img_W×Img_H，首先得到相机所能观察到的图像范围，以左相机图像生成为例，在VC中用下面的程序实现：
glViewport(0，0，Img_W，Img_H)；
glMatrixMode(GL_PROJECTION)；
glLoadIdentity()；
gluPerspective(FOV，Img_W/Img_H，1，100000)；
glMatrixMode(GL_MODELVIEW)；
………
m_GL.SetLighting()；
glLoadMatrixd(&m_Chaser.m_MatrixTmp1[0][0])；
m_Cam.ConfigerCamera()；
glPushMatrix()；
m_Target.Draw()；
glPopMatrix()；
m_Chaser.Draw()；
m_Arm.Draw()；
m_Scene.Draw()；
执行上述程序后，左相机所能观察到的图像存在在缓存中，该图像仍然是24位彩色图像，需要转换为8位的黑白图像，叠加上图像噪声后，存储在图像矩阵中：
       glReadBuffer(GL_BACK)；
       glReadPixels(0，0，Img_W，Img_H，GL_RED，GL_UNSIGNED_BYTE，ImageDataPixels)；
       for(j＝0；j＜Img_H；j++)/*----单通道的图----相当于8位图---*/
       {
            for(i＝0；i＜IMG_WIDTH；i++)
            {
                g_Img_L[i][j]＝ImageDataPixels[i*Img_H+j]+ImgNoise[i][j]；
            }
       }
ImgNoise是采用高斯随机数函数生成的随机数矩阵，用于模拟图像噪声。由此，实现了左相机的图像生成与采集，得到含噪声的图像矩阵g_Img_L，类似的可得到含噪声的右相机图像矩阵g_Img_R。该两矩阵作为图像处理与位姿测量模块的输入。典型经过图像生成与采集得到的的左、右相机图像，分别如图21、图22所示。
七、立体相机标定
标定图像生成与采集的实现与上面介绍的步骤相同，在此介绍标定的算法。针孔成像模型(1)可改写为：
$Z_{\mathrm{ci}}\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]=\left[\begin{array}{cccc}{m}_{11}& m_{12}& m_{13}& m_{14}\\ m_{21}& m_{22}& m_{23}& m_{24}\\ m_{21}& m_{32}& m_{33}& m_{34}\end{array}\right]\left[\begin{array}{c}{X}_{\mathrm{wi}}\\ Y_{\mathrm{wi}}\\ Z_{\mathrm{wi}}\\ 1\end{array}\right]---\left(35\right)$
式中，(Xwi，Ywi，Zwi，1)为空间第i点的坐标；(ui，vi，1)为第i点的图像坐标；mij为投影矩阵M的第i行j列元素。式(35)包含三个方程：
$\left\{\begin{array}{c}{Z}_{\mathrm{ci}}{u}_i=m_{11}{X}_{\mathrm{wi}}+m_{12}{Y}_{\mathrm{wi}}+m_{13}{Z}_{\mathrm{wi}}+m_{14}\\ Z_{\mathrm{ci}}{v}_i=m_{21}{X}_{\mathrm{wi}}+m_{22}{Y}_{\mathrm{wi}}+m_{23}{Z}_{\mathrm{wi}}+m_{24}\\ Z_{\mathrm{ci}}=m_{31}{X}_{\mathrm{wi}}+m_{32}{Y}_{\mathrm{wi}}+m_{33}{Z}_{\mathrm{wi}}+m_{34}\end{array}\right.---\left(36\right)$
将式(36)中的第一式除以第三式，第二式除以第三式分别消去Zci后，可得到2个关于mij的线性方程：
$\left\{\begin{array}{c}{X}_{\mathrm{wi}}{m}_{11}+Y_{\mathrm{wi}}{m}_{12}+Z_{\mathrm{wi}}{m}_{13}+m_{14}-u_i{X}_{\mathrm{wi}}{m}_{31}-u_i{Y}_{\mathrm{wi}}{m}_{32}-u_i{Z}_{\mathrm{wi}}{m}_{33}=u_i{m}_{34}\\ X_{\mathrm{wi}}{m}_{21}+Y_{\mathrm{wi}}{m}_{22}+Z_{\mathrm{wi}}{m}_{23}+m_{24}-v_i{X}_{\mathrm{wi}}{m}_{31}-v_i{Y}_{\mathrm{wi}}{m}_{32}-v_i{Z}_{\mathrm{wi}}{m}_{33}=v_i{m}_{34}\end{array}\right.---\left(37\right)$
上式表示，如果标定块上有N个已知点，并已知它们的空间坐标(Xwi，Ywi，Zwi)与它们的图像点坐标(ui，vi)，则有2N个关于C矩阵元素的线性方程，下面用矩阵形式表示这些方程：
$\left[\begin{array}{ccccccccccc}{X}_{w1}& Y_{w1}& Z_{w1}& 1& 0& 0& 0& 0& -u_1{X}_{w1}& -u_1{Y}_{w1}& -u_1{Z}_{w1}\\ 0& 0& 0& 0& X_{w1}& Y_{w1}& Z_{w1}& 1& -v_1{X}_{w1}& -v_1{Y}_{w1}& -v_1{Z}_{w1}\\ & & & & ...& & ...& & ...& & \\ & & & & & & & & & & \\ & & & & ...& & ...& & ...& & \\ & & & & & & & & & & \\ & & & & ...& & ...& & ...& & \\ & & & & ...& & ...& & ...& & \\ & & & & & & & & & & \\ X_{\mathrm{wn}}& Y_{\mathrm{wn}}& Z_{\mathrm{wn}}& 1& 0& 0& 0& 0& -u_1{X}_{\mathrm{wn}}& -u_1{Y}_{\mathrm{wn}}& -u_1{Z}_{\mathrm{wn}}\\ 0& 0& 0& 0& X_{\mathrm{wn}}& Y_{\mathrm{wn}}& Z_{\mathrm{wn}}& 1& -v_1{X}_{\mathrm{wn}}& -v_1{Y}_{\mathrm{wn}}& -v_1{Z}_{\mathrm{wn}}\end{array}\right]\times \left[\begin{array}{c}{m}_{11}\\ m_{12}\\ m_{13}\\ m_{14}\\ m_{21}\\ m_{22}\\ m_{23}\\ m_{24}\\ m_{31}\\ m_{32}\\ m_{33}\end{array}\right]=\left[\begin{array}{c}{u}_1{m}_{34}\\ v_1{m}_{34}\\ ...\\ ...\\ ...\\ ...\\ ...\\ ...\\ ...\\ u_n{m}_{34}\\ v_n{m}_{34}\end{array}\right]---\left(38\right)$
由(1)式可见，C矩阵乘以任意不为零的常数并不影响(Xw，Yw，Zw)与(u，v)的关系，因此上式中可以指定m34＝1，从而得到关于M矩阵其他元素的2N个线性方程，这些未知元素的个数为11个，记为11维向量m，将方程组简写成：
Km＝U                 (39)
其中，K为式(38)左边2N×11矩阵，m为未知的11维向量，U为式(38)右边的2N维向量，K，U为已知向量，当2N＞11时，我们可以用最小二乘法求出上述线性方程组的解为：
m＝(KTK)-1KTU         (40)
m向量与m34＝1构成了所求解的C矩阵，由上可见，由空间6个或6个以上已知点与它们的图像点坐标，就可以求出C矩阵。在标定中，标定版上有7×7个已知点，且采集了不同位置、姿态下的21幅图，使方程的个数大大超过未知数的个数，从而用最小二乘法求解以降低误差造成的影响。
对左、右相机，均采用如上的标定算法，分别得到标定矩阵CL、CR。