专利名称：：空间机器人非合作目标自主识别与捕获方法
技术领域：
：本发明涉及一种空间机器人非合作目标自主识别与捕获方法，属于空间机器人在轨服务
技术领域：
。
背景技术：
：随着技术的进步，人类的活动在不断地向太空扩展。据统计，全球平均每年发射80-130颗卫星，然而有2-3颗卫星未能正确入轨，而正确入轨的卫星中，又有5-10颗在寿命初期(入轨后前30天)即失效，其中，机械故障导致的卫星失效占了相当大的比例(TafazoliM.Astudyofon_orbitspacecraftfailures[J].ActaAstronautica.2009，64:195-205)。典型的例子如我国的鑫诺二号卫星，于2006年IO月29日发射升空后，卫星虽然成功定轨，且卫星的测控、姿轨控等分系统均处于良好的状态，然而，由于太阳帆板二次展开和天线展开未能完成，卫星无法正常工作，虽然科研人员采用多种方法进行营救，仍然未能恢复其功能，这颗耗资20亿人民币、设计寿命为15年的卫星随即成了一颗废星。对于那些完全失效或由于任务结束被放弃的卫星，停留在太空将成为太空垃圾，不但占用了宝贵的轨道资源，还可能危机其它航天器的安全。为了尽可能挽回损失或净化轨道环境，各国正在研究以空间机器人为手段，以卫星维修、生命延长及太空垃圾清除为目的的在轨服务技术(崔乃刚，王平，郭继峰，等.空间在轨服务技术发展综述[J].宇航学报.2007，28(4):33-39.)。对于大多数已在轨服务的航天器和空间碎片，有三个特点1)没有安装用于机械臂捕获的抓持机构(手柄)以及用于辅助测量的合作标志器和特征块等；2)目标星运动状态未知，可能为三轴稳定、自旋稳定甚至是失控状态下的翻滚等；3)目标星与追踪星之间没有直接的信息交流。此类目标称为非合作目标。为了将空间机器人更好地用于在轨服务，必须解决对非合作目标的自主识别与抓捕的关键问题。同时，在空间对抗中，为了监视、破坏、"俘虏"敌方卫星，也需要解决非合作目标的自主识别与捕获问题。非合作目标的识别与捕获是一个世界性难题，已经引起了国内外研究者的重视。文献(张世杰，曹喜滨，闽陈.非合作航天器间相对位姿的单目视觉确定算法[J].南京理工大学学报.2006,30(5):564-568)提出了一种不采用合作光标情况下的位姿测量方法，但假设被识别目标的形状及几何尺寸已知。欧空局设计了地球静止轨道恢复器ROGER(RoboticGeostationaryOrbitRestorer),使用绳系飞网或飞爪，对轨道上的废弃卫星进行抓捕(D.A.Smith,C.Martin,M.Kassebom，H.Petersen,A.Shaw，B.Skidmore，D.Smith,H.Stokes，A.Willig."Amissiontopreservethegeostationaryregion",AdvancesinSpaceResearch34(2004)1214-1218)，其中对目标的测量使用了包括激光测距、主动视觉等手段。文献(ThienelJK，Vane印oelJM，Sa皿erRM.Accuratestateestimationandtrackingofanon_cooperativetargetvehi-cle[C].AIAAGuidance,Navigation,andControlConfer-ence，Keystone,C0，UnitedStates,AIAA2006-6802,2006:5511-5522.)针对哈勃太空望远镜的服务，提出了一种非线性的方法用于估计航天器的姿态，并进行跟踪控制，但5采用了较多的先验知识。文献(InabaN，0daM，AsanoM.Rescuingastrandedsatelliteinspace-experimentalroboticc邻tureofnon_cooperativesatel1ites[J].TransactionsoftheJapanSocietyforAero—nauticalandSpaceSciences.2006，48(162):213-220.)提出了一种对非合作目标在轨识别与捕获的方法，但假定目标的外形、尺寸及质量已知。目前DLR正在开展的DEOS项目(KlausLandzettel，AlinAlbu_Sch姐er，BernhardBr皿ner，etal.R0KVISS:VerificationofAdvancedLightWeightRoboticJointsandTele—PresenceConceptsforFutureSpaceMissions,ICRA2008,Pasadena,California,USA)，采用了复杂的非合作目标识别算法，但由于受星载处理器计算能力的限制，该算无法在星上自主完成，而是将采集的图像下传到地面，在地面操作员的直接参与下，由地面设备进行图像处理并计算目标位姿，再将测量结果上传到星上，星上控制器控制追踪星对目标进行跟踪、接近等操作。该方法受到传输时延、传输可靠度的影响较大，时延较大时整个系统将不稳定；而且由于需要操作人员的直接参与，其"自主"性不高。因此，提出一种空间机器人非合作目标自主识别与捕获方法，是非常必要和迫切的。
发明内容本发明的目的在于克服现有技术的不足，提供一种空间机器人非合作目标自主识别与捕获方法，主要步骤包括基于立体视觉的图像处理与非合作目标相对位置测量、空间机器人目标捕获自主路径规划，以及空间机器人系统协调控制，该方法不要求在目标星上安装用于辅助测量的发光标志器或角反射镜，也无需知道目标的几何尺寸，而是直接以航天器自身的部件作为识别对象，这些部件可以是帆板支架、天线支架、远地点发动机，也可以是星箭对接环等。本发明的空间机器人非合作目标自主识别与捕获方法，包括如下步骤(1)基于立体视觉的图像处理与非合作目标相对位置测量，根据同步采集到的左、右相机图像，进行实时处理，包括平滑滤波、边沿检测、直线提取、目标特征识别、立体匹配与3D重构。基于重构的结果，最后计算出非合作目标星相对于空间机器人基座和末端的相对位置&和姿态t;(2)空间机器人目标捕获自主路径规划，根据相对位姿测量结果，即相对位置/L和姿态^，实时规划空间机器人各关节的运动轨迹——关节角Od和角速度A，以使机械臂末端接近并最终捕获目标；(3)空间机器人系统协调控制，将空间机械臂各关节的控制与基座的姿态、轨道控制协调进行，控制机械臂各关节跟踪期望的位置d和速度^外，实现整个系统的最佳控制性能。所述的基于立体视觉的图像处理与非合作目标相对位置测量，包括以下步骤(1)图像滤波分别对左右相机图像进行滤波，以消除噪声干扰，得到平滑的左右相机图像；(2)边沿检测对两幅图像分别进行边沿检测，得到边沿特征信息；(3)直线提取对进行边沿检测后的图像进行直线提取，得到包含三角形支架各直线在内的各条直线信息；6(4)非合作目标卫星三角支架的识别从提取后的所有直线信息中，识别对应于三角形帆板支架的六条直线，并利用识别出来的三角形支架对应的六条直线，计算出三角形的顶点；(5)立体匹配、3D重构与位姿测量根据识别出的三角形支架各顶点分别在左、右相机中的2D信息，进行各特征点的3D重构，得到三角形支架各顶点在世界坐标系中的3D坐标；根据3D重构的结果，进一步构建抓捕对象坐标系，并计算出其相对于世界坐标系的位置和姿态。所述的空间机器人目标捕获自主路径规划，包括以下步骤(1)位姿偏差计算，将立体视觉测量的目标相对于基座的位姿测量数据，转换到目标相对于机械手末端的位姿，并判断相对位姿偏差ep和e。是否小于设定的阈值s和e。，若小于，则闭合手爪、捕获目标；否则，执行下面其它步骤；(2)目标运动的预测，根据相对位姿偏差，实时估计目标的运动状态——下一个时刻的位置、姿态及线速度、角速度；(3)空间机器人末端运动速度规划，根据所预测的目标运动状态，规划机械臂末端的运动速度，以与最短路径捕获目标；(4)空间机器人避奇异的路径规划，根据规划的末端运动速度，采用奇异回避的处理方法，规划机械臂各关节角的运动轨迹。所述的空间机器人系统协调控制使"机械臂控制器"和"基座控制器"协同工作，实现整个系统控制性能的优化。机械臂控制系统控制机械臂的运动，并且估计机械臂运动对于卫星的反作用，以力矩或者角动量的形式传给卫星基座的姿轨控系统AOCS，由A0CS对该反作用进行补偿，使卫星的姿态保持稳定，同时姿态测量元件将姿态控制状态——姿态角、姿态角速率、飞轮和喷气的状态又传给机械臂控制系统，机械臂系统根据这些状态判断运动规划，以决定是否重新规划，如果姿态角、角速度超出预设的范围，说明规划的路径不好，需要重新规划。基座的控制系统除了根据姿态测量状态进行常规的反馈控制以外还根据估计出的机械臂运动对卫星的反作用进行前馈补偿；所述的图像滤波采用中值滤波算法、所述的边沿检测采用Ca皿y算法、所述的直线提取采用Hough变换算法。中值滤波对于给定的n个数值{ai，a2，…，aj，将它们按大小顺序排列，当n为奇数时，位于中间位置的那个数值称为这n个数值的中值。当n为偶数时，位于中间位置的两个数值的平均值称为这n个数值的中值，记作med{ai，d2，…，an}，图像经过中值滤波后，某像素的输出等于该像素领域中的各像素灰度的中值；Ca皿y边缘检测算法用高斯滤波器平滑图像、用一阶偏导的有限差分来计算梯度的幅值和方向、对梯度幅值应用非极大值抑制、用双阈值算法检测和连接边缘；Hough变换算法用点_线的对偶性，将图像空间x-y中的直线，变换成为参数空间的一点；所述的非合作目标卫星三角支架的识别包括初始捕获与实时跟踪两个主要步骤，初始捕获通过对遥测通道下传到地面的左、右相机图像设置两个初始参考点Pri、Pd，星载程序即可完成对帆板支架的自动识别；初始捕获建立后，星载程序随即转入跟踪模式，以后能够自动根据图像，实时跟踪帆板支架所在的区域。参考点Pri为三角形内的任意一点，Prt为三角形外且位于太阳电池阵列上的任意一点，Pri、通过鼠标直接在遥测图像中选取，没有特别严格的要求；所述的图像处理与非合作目标相对位置测量，其特征在于所述的立体匹配、3D重构与位姿测量步骤利用从左、右相机图像中分别识别出的三角形支架顶点的图像坐标，采用最小二乘法对立体投影模型进行计算，得到三角形支架顶点在世界坐标系中的3D坐标，并根据此三点的坐标构建目标坐标系，计算目标坐标系相对于世界坐标系的位置、姿态；所述的空间机器人目标捕获自主路径规划，其特征在于所述的目标运动的预测步骤首先建立目标的运动的状态方程，然后采用扩展Kalman滤波器预测目标卫星下一个时刻的相对位置、姿态及相对线速度、角速度；所述的空间机器人目标捕获自主路径规划，其特征在于所述的空间机器人末端运动速度规划步骤根据下式规划机械臂末端的运动速度(1)<formula>formulaseeoriginaldocumentpage8</formula>上式中，EVed、E"ed分别为规划的末端线速度和角速度；K为增益矩阵，为对角阵，用于限制规划的末端最大速度；ErA、Evh、E"h分别为预测的目标下一个时刻的相对位置、线速度和角速度，在机械臂末端坐标系中的表示，AO为姿态指向误差，按下式计算(2)AO-告(wwflexflA)=|:、(2，3)-、(3,2)-、(1,3)《(3'1)乂(1，2)-、(2，1)其中，[ne，oe，aj和[nh，oh，ah]分别为机械臂末端坐标系和目标坐标系的姿态变换矩阵，而EAH为通过手眼相机测出的手柄相对于末端坐标系的姿态矩阵。所述的空间机器人目标捕获自主路径规划，其特征在于所述的空间机器人避奇异的路径规划将空间机器人的动力学奇异回避转化为实时的运动学奇异回避，艮卩(3)<formula>formulaseeoriginaldocumentpage8</formula><formula>formulaseeoriginaldocumentpage8</formula>上式左边为机械臂末端相对于基座的运动速度(在基座坐标系中的表示)，因而<formula>formulaseeoriginaldocumentpage8</formula>(4)Eve°、E"e°分别表示机械臂末端相对于基座的线速度、角速度在基座坐标系中的表示^之为机械臂的普通Jacobian矩阵，由此，根据(4)求解期望的关节角速度^^。本发明与现有技术相比具有如下优点(l)采用的非合作目标自主识别与位姿测量，不要求在目标星上安装用于辅助测量的发光标志器或角反射镜，也无需知道目标的几何尺寸，直接识别航天器自身的部件，并计算相对位姿；(2)采用的空间机器人目标捕获自主路径规划方法，能实时预测目标的运动状态、自主回避运动学及动力学奇异、实时预测机械臂运动对基座产生的干扰并自主调整规划的路径；(3)采用的协调控制方法，既考虑了整个空间机器人系统的控制性能，又适应了当前的星载处理器的控制能力。图1是典型的空间机器人在轨服务流程；图2是空间机器人系统及非合作目标在轨捕获示意图；图3是空间机器人非合作目标自主识别与捕获方法流程图；图4是基于立体视觉的图像处理与非合作目标相对位置测量算法流程；图5是模拟生成的虚拟左相机图像(512X512黑白)；图6是模拟生成的虚拟右相机图像(512X512黑白)；图7是所识别出的非合作目标的帆板支架；图8是交会参考坐标系与捕获点坐标系的定义；图9是空间机器人捕获非合作目标的自主路径规划算法流程；图10是空间机器人捕获非合作目标的协调控制方法结构。具体实施例方式—、空间机器人非合作目标自主识别与捕获方法主要流程典型的空间机器人在轨服务思想如图1所示，即利用空间机器人系统，对故障卫星进行跟踪、接近、捕获、对接、维修等操作。在轨服务流程大致可分为如下几个阶段(a)远距离跟踪接近(>300m):空间机器人系统从自身轨道机动到距离目标卫星300m左右的位置；(b)中距离跟踪接近空间机器人系统从300m机动到距离目标卫星15m左右的位置；(c)近距离交会与停靠空间机器人系统从15m左右的位置机动到lm左右的位置，并进行相对停靠，使得目标卫星处于机械臂的工作空间内；(d)在轨抓捕与对接利用空间机械手抓捕目标卫星，并将其与空间机器人基座对接在一起；(e)在轨维修或执行离轨操作，即将目标带入地球大气层或坟墓轨道利用机械臂对目标卫星进行在轨维修，或者将目标带入地球大气层或坟墓轨道。假设待服务对象为一帆板未展开的故障卫星，其上未预先安装用于视觉测量的标志器和用于捕获的手柄，即为非合作目标。所设计的空间机器人在轨服务系统由一飞行基座和机械手组成，如图2所示。其中，飞行基座上安装了目标测量系统——基座立体视觉、对接机构、姿轨控系统等，空间机械手由6D0F机械臂、抓捕手爪及末端立体视觉组成。为实现空间机器人系统对故障卫星进行跟踪、接近、抓捕、维修等操作，采用如图3所示的非合作目标自主识别与捕获方法，主要步骤包括(1)基于立体视觉的图像处理与非合作目标相对位置测量，根据同步采集到的左、右相机图像，进行实时处理，包括平滑滤波、边沿检测、直线提取、目标特征识别、立体匹配与3D重构。基于重构的结果，最后计算出非合作目标星相对于空间机器人基座和末端的相对位置^和姿态t;(2)空间机器人目标捕获自主路径规划，根据相对位姿测量结果，及相对位置&和姿态t，实时规划空间机器人各关节的运动轨迹——关节角Od和角速度《，以使机械臂末端接近并最终捕获目标；(3)空间机器人系统协调控制，将空间机械臂各关节的控制与基座的姿态、轨道控9制协调进行，控制机械臂各关节跟踪期望的位置d和速度A外，实现整个系统的最佳控制性能。二、基于立体视觉的图像处理与非合作目标相对位置测量基于立体视觉的图像处理与非合作目标相对位置测量算法对同步采集到的左、右相机图像进行处理，并计算出目标相对于基座的位置和姿态，流程如图4所示。以如图5和图6所示的左、右相机模拟图像为例，主要步骤如下(1)图像滤波分别对左右相机图像进行滤波，以消除噪声干扰，得到平滑的左右相机图像实际获得的图像一般都含有噪声，本发明采用中值滤波算法进行图像平滑。中值滤波是一种抑制噪声的非线性处理方法。对于给定的n个数值{ai，a2，…，aj，将它们按大小顺序排列。当n为奇数时，位于中间位置的那个数值称为这n个数值的中值。当n为偶数时，位于中间位置的两个数值的平均值称为这n个数值的中值，记作med{ai，a2，…，an}。图像经过中值滤波后，某像素的输出等于该像素领域中的各像素灰度的中值。中值滤波的方法运算简单，易于实现，而且能较好地保护边界。(2)边沿检测对两幅图像分别进行边沿检测，得到边沿特征信息边缘(edge)是指图像局部强度变化最显著的部分。边缘主要存在于目标与目标、目标与背景、区域与区域(包括不同色彩)之间，是图像分割、纹理特征和形状特征等图像分析的重要基础。本文采用Ca皿y算法对图像进行边缘检测。Ca皿y边缘检测算法的步骤如下(a)用高斯滤波器平滑图像；(b)用一阶偏导的有限差分来计算梯度的幅值和方向；(c)对梯度幅值应用非极大值抑制；(d)用双阈值算法检测和连接边缘。(3)直线提取对进行边沿检测后的图像进行直线提取，得到包含三角形支架各直线在内的各条直线信息在利用Ca皿y算法检测出图像边沿后，再采用Hough变换提取直线。哈夫变换的基本思想是利用点-线(点-直线或点-曲线)的对偶性，即图像空间x-y中的直线，经过Hough变换后成为参数空间的一点。Hough变换算法的步骤如下(a)适当地量化参数空间；(b)假定参数空间的每一个单元都是一个累加器，把累加器初始化为零；(c)对图像空间的每一点，在其所满足的参数方程对应的累加器上加1;(d)累加器阵列的最大值对应模型的参数。(4)三角支架的识别从提取后的所有直线信息中，识别对应于三角形帆板支架的六条直线采用Hough变换后，可以提取出多条直线，包括目标卫星的轮廓、太阳电池阵列及太阳帆板支架。由于帆板支架有明显的特征，故以帆板支架为识别对象。从众多直线中识别出属于帆板支架的六条直线，是本文研究的关键。由于除了已知帆板支架是一三角形外，没有其他任何先验知识——包括三角形的边长、内角、各边的倾斜度等，因此，识别的难度可想而知。在此假定图像可通过遥测通道下传到地面，地面操作员根据该图像设置两个初始参考点Pri、Pw星载程序即可完成对帆板支架的自动识别，随即转入跟踪模式，以后能够自动根据图像，实时跟踪帆板支架所在的区域。参考点P^为三角形内的任意一点，P^为三角形外且位于太阳电池阵列上的任意一点，Pri、通过鼠标直接在遥测图像中选取，没有特别严格的要求。识别的原理及结果如图7所示。10(5)交点特征的提取利用识别出来的三角形支架对应的六条直线，计算出三角形的顶点(6)立体匹配、3D重构根据识别出的三角形支架各顶点分别在左、右相机中的2D信息，进行各特征点的3D重构，得到三角形支架各顶点在世界坐标系中的3D坐标，即、WP6:首先，相机成像用针孔模型可表示为(5)义1其中，WP=[Xw，Yw，ZW]T为空间某点P在世界坐标系中的坐标，(u，v)是以像素为单位的图像坐标，C为相机的投影变换矩阵，由其内外参数确定。基于立体视觉的3D重建原理如图7所示，假定第i个特征点Pi在世界坐标系中的位置为wPi二[XWiYWiZJT，投影在左右相机的图像坐标分别为Pu=[UuVli]lPpKi=[uKivKi]T。根据(5)，对于左、右相机，有如下关系1113i为对应的元素。对式(6)和(7)Z,..(8)矩阵C。Q分别为左右相机的变换矩阵，a进行化简，有1—"8W"^-"9"丄,"2-"0"丄,"4一"8V""5—"9V"WliA-toA-to,.写成矩阵的形式KwPi=U(9)式(8)有四个方程、三个未知数，为超越方程，可用最小二乘法进行求解wPi=(KTK)-(10)上述过程即为基于立体视觉的3D重构过程。(7)目标位置、姿态的测量根据3D重构的结果，进一步构建抓捕对象坐标系，并计算出其相对于世界坐标系——追踪星测量坐标系的位置和姿态。根据已识别出的六个特征点，可建立目标的"交会参考坐标系"EK2和"捕获点坐标系"E^，如图8所示。首先，支架中心的三个点AQ3)的三维坐标由下面几式确定:，V'+ffP2(11):271a4fAAAAAA1AAii_---扁々义i々一l11则中点、>W2坐标系E^的原点为坐标系E^的X轴为单位矢3(12)(13)(14)(15)坐标系E^的Y轴为单位矢3(16)(17)的姿态旋转矩阵为剩坐标系EK2的Z轴由右手定则确定aK2=nK2XoK2(18)则坐标系EK2相对于交会参考系EK1AK2=[nK2oK2aK2](19)姿态欧拉角可通过姿态旋转矩阵WA^求出，结合式(15)求出的相对位置，至此，目标的相对位置和姿态均已测出。类似的，"捕获点坐标系"E^相对于"交会参考系"EK1的位置、姿态亦可计算出。三、空间机器人目标捕获自主路径规划上述图像处理与位姿测量算法给出了"捕获点坐标系"E^相对于"交会参考系"EK1的位置、姿态，自主规划算法首先将该测量结果先转化为"捕获点坐标系"E^相对于"机械臂末端坐标系"的位置、姿态，然后根据此结果，实时规划空间机器人的运动，以最终捕获目标。主要包括如图9所示，包括手眼相机测量、位姿偏差计算、目标运动的预测、空间机器人末端运动速度规划、空间机器人避奇异的路径规划等。首先，根据手眼测量数据判断相对位姿偏差ep和e。是否小于设定的阈值％和e。(即捕获区域，如三轴相对位置在10mm以内，三轴姿态偏差在1°左右)，若小于，则闭合手爪、捕获目标；反之，则根据相对位姿偏差，实时估计目标的运动状态，并将估计的结果反应到机械臂末端速度的规划中，以保证机械臂末端时刻朝最近的方向(直线)趋近目标，机械臂末端能自主跟踪目标的运动，直到最后捕获目标。规划出末端运动速度后，即调用自主奇异回避算法，以解算关节的期望角采用Kalman滤波器预测目标下一个时刻的相对位置、姿态及相对线速度、角速度，基于预测的结果，期望的末端运动速度按下式进行规划(20)<formula>formulaseeoriginaldocumentpage12</formula>0104]上式中，Evh、E"h为目标的绝对运动速度在Ee中的表示，AO为姿态指向误差(即手柄坐标系相对于机械臂末端坐标系的姿态)，按下式计算(21)0105]A。:=|("eX"A+0eXwaA)=I、(2，3)-、(3,2)-、(1,3)"(3,1)乂(1,2)-、(2,1)—0106]其中，[ne，，aj和[nh，oh，ah]分别为Ee和Eh的姿态变换矩阵(相对于惯性系)，而EAH为通过手眼相机测出的手柄相对于末端坐标系的姿态矩阵。根据相对运动原理，目标的绝对运动速度通过下面的式子求出0107]0108]0109]示，即0110]0112]0113]0114]0115]0116]入(26)0117]0118]:0119]:0120]:0121](22)(23)其中，N^表示目标星的手柄相对于机械臂末端的线速度，在末端坐标系中的表(24)0-&0c"^10(25)根据式(22)和(23)，目标的运动速度、+'3x3—五,_、3x3'3x3—(26)根据自由漂浮空间机器人的微分运动学方程，有(27)其中，Ejg为机械臂的末端广义雅可比矩阵，6为机械臂的当前角速度，将式(27)代,有(28)、；+"^3x3-V、o3x3J3x3_最后，根据式(28)和(l)，可按下式规划机械臂末端运动速度4、AO+:五e+'3x33x33x3(29)另一方面，空间机器人的微分运动学方程可表示为、o+(30):0122]经过适当的修改后，文献(徐文福，斌，刘宇，李成，强文义.一种新的PUMA类型机器人奇异回避算法[J].自动化学报.2008,34(6):670-675)中介绍的自由漂浮空间机器人奇异回避算法，可用于自主路径规划的奇异回避。首先，在末端坐标系中表示的腕部运动速13度与末端运动速度之间的关系为一(T《(w0=、-=、-《—00(32)其次，机械臂腕部雅可比矩阵i,，o射/21t/22.(33)力i(&23W6+^6)(淋-W6)p4+4，64(，6-站)^V^23+《Q1*^4*^5(<*V5C6+￡)一勺i—+C4C6、6&o_(36)./一-C5c600、可推导出6A_(c4c5w6)W55455C5=[/ioa]则同理—C606爿=一^0001(38)(39)(31)(34)(35)(37)14<table>tableseeoriginaldocumentpage15</column></row><table>由此，可将文献(徐文福，斌，刘宇，李成，强文义.一种新的PUMA类型机器人奇异回避算法[J].自动化学报.2008,34(6):670-675)的奇异回避算法用于目标捕获的自主路径规划。四、空间机器人系统协调控制当期望的基座姿态角及机械臂关节角轨迹规划出来后，需要设计相应的控制策略，控制基座姿态及机械臂关节角跟踪所规划的轨迹。传统的方式是将基座及机械臂两部分各自独立进行控制，即独立设计各自的控制器，如基座姿态和关节角分别按如下PD控制律进行控制卜6=、。(《d—A)+、。(aw—w。)然而，这种控制方式下，"机械臂控制系统"与"基座控制系统"之间各自为政，很难实现整个系统控制性能的优化，如所规划的机械臂的运动可能产生超出基座控制能力的干扰，导致基座姿态控制器无法实现期望的姿态变化，反过来又影响机械臂末端的定位。同时，在现有条件下，设计能同时控制基座姿态及机械臂关节角的控制器是不可能，因此，本文考虑采用协调控制的方法，从硬件上虽然仍分为"机械臂控制系统"和"基座控制系统"，但他们的控制行为是"相互协调"的。其中机械臂控制系统控制机械臂的运动，并且估计机械臂运动对于卫星的反作用，以力矩或者角动量的形式传给卫星基座的姿轨控系统AOCS，由AOCS对该反作用进行补偿，使卫星的姿态保持稳定，同时姿态测量元件将姿态控制状态又传给机械臂控制系统，机械臂系统根据这些状态判断运动规划，以决定是否重新规划，如果姿态角、角速度超出预设的范围，说明规划的路径不好，需要重新规划。基座的控制系统除了根据姿态测量状态进行常规的反馈控制以外还根据估计出的机械臂运动对卫星的反作用进行前馈补偿。这样就实现了卫星姿态和机械臂运动的协调控制，这是一种分布式和协作式的控制方法。协调控制的思想如图10中蓝色虚框所示。本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。1权利要求空间机器人非合作目标自主识别与捕获方法，其特征在于包括以下步骤(1)基于立体视觉的图像处理与非合作目标相对位置测量1，根据同步采集到的左、右相机图像，进行实时处理，包括平滑滤波、边沿检测、直线提取、目标特征识别、立体匹配与3D重构。基于重构的结果，最后计算出非合作目标星相对于空间机器人基座和末端的相对位置和姿态(2)空间机器人目标捕获自主路径规划2，根据相对位姿测量结果，即相对位置和姿态实时规划空间机器人各关节的运动轨迹——关节角和角速度以使机械臂末端接近并最终捕获目标；(3)空间机器人系统协调控制3，将空间机械臂各关节的控制与基座的姿态、轨道控制协调进行，控制机械臂各关节跟踪期望的位置和速度外，实现整个系统的最佳控制性能。F2009100734714C00011.tif,F2009100734714C00012.tif,F2009100734714C00013.tif,F2009100734714C00014.tif,F2009100734714C00015.tif,F2009100734714C00016.tif,F2009100734714C00017.tif,F2009100734714C00018.tif2.根据权利要求1所述的空间机器人非合作目标自主识别与捕获方法，其特征在于所述的基于立体视觉的图像处理与非合作目标相对位置测量包括以下步骤(1)图像滤波分别对左右相机图像进行滤波，以消除噪声干扰，得到平滑的左右相机图像；(2)边沿检测对两幅图像分别进行边沿检测，得到边沿特征信息；(3)直线提取对进行边沿检测后的图像进行直线提取，得到包含三角形支架各直线在内的各条直线信息；(4)非合作目标卫星三角支架的识别从提取后的所有直线信息中，识别对应于三角形帆板支架的六条直线，并利用识别出来的三角形支架对应的六条直线，计算出三角形的顶点；(5)立体匹配、3D重构与位姿测量根据识别出的三角形支架各顶点分别在左、右相机中的2D信息，进行各特征点的3D重构，得到三角形支架各顶点在世界坐标系中的3D坐标；根据3D重构的结果，进一步构建抓捕对象坐标系，并计算出其相对于世界坐标系的位置和姿态。3.根据权利要求1所述的空间机器人非合作目标自主识别与捕获方法，其特征在于所述的空间机器人目标捕获自主路径规划包括以下步骤(1)位姿偏差计算，将立体视觉测量的目标相对于基座的位姿测量数据，转换到目标相对于机械手末端的位姿，并判断相对位姿偏差ep和e。是否小于设定的阈值％和e。，若小于，则闭合手爪、捕获目标；否则，执行下面其它步骤；(2)目标运动的预测，根据相对位姿偏差，实时估计目标的运动状态——下一个时刻的位置、姿态及线速度、角速度；(3)空间机器人末端运动速度规划，根据所预测的目标运动状态，规划机械臂末端的运动速度，以与最短路径捕获目标；(4)空间机器人避奇异的路径规划，根据规划的末端运动速度，采用奇异回避的处理方法，规划机械臂各关节角的运动轨迹。4.根据权利要求1所述的空间机器人非合作目标自主识别与捕获方法，其特征在于所述的空间机器人系统协调控制使"机械臂控制器"和"基座控制器"协同工作，实现整个系统控制性能的优化。机械臂控制系统控制机械臂的运动，并且估计机械臂运动对于卫星的反作用，以力矩或者角动量的形式传给卫星基座的姿轨控系统AOCS，由AOCS对该反作用进行补偿，使卫星的姿态保持稳定，同时姿态测量元件将姿态控制状态——姿态角、姿态角速率、飞轮和喷气的状态又传给机械臂控制系统，机械臂系统根据这些状态判断运动规划，以决定是否重新规划，如果姿态角、角速度超出预设的范围，说明规划的路径不好，需要重新规划。基座的控制系统除了根据姿态测量状态进行常规的反馈控制以外还根据估计出的机械臂运动对卫星的反作用进行前馈补偿；5.根据权利要求2所述的图像处理与非合作目标相对位置测量，其特征在于所述的图像滤波采用中值滤波算法、所述的边沿检测采用Ca皿y算法、所述的直线提取采用Hough变换算法，中值滤波对于给定的n个数值{ai，a2，…，aj，将它们按大小顺序排列。当n为奇数时，位于中间位置的那个数值称为这n个数值的中植。当n为偶数时，位于中间位置的两个数值的平均值称为这n个数值的中值，记作med{ai，a2，…，an}，图像经过中值滤波后，某像素的输出等于该像素领域中的各像素灰度的中值；Ca皿y边缘检测算法用高斯滤波器平滑图像、用一阶偏导的有限差分来计算梯度的幅值和方向、对梯度幅值应用非极大值抑制、用双阈值算法检测和连接边缘；Hough变换算法用点-线的对偶性，将图像空间x-y中的直线，变换成为参数空间的一点；6.根据权利要求2所述的图像处理与非合作目标相对位置测量，其特征在于所述的非合作目标卫星三角支架的识别包括初始捕获与实时跟踪两个主要步骤，初始捕获通过对遥测通道下传到地面的左、右相机图像设置两个初始参考点Pri、Pr"星载程序即可完成对帆板支架的自动识别；初始捕获建立后，星载程序随即转入跟踪模式，以后能够自动根据图像，实时跟踪帆板支架所在的区域。参考点Pd为三角形内的任意一点，P^为三角形外且位于太阳电池阵列上的任意一点，Pri、Prt通过鼠标直接在遥测图像中选取，没有特别严格的要求；7.根据权利要求2所述的图像处理与非合作目标相对位置测量，其特征在于所述的立体匹配、3D重构与位姿测量步骤利用从左、右相机图像中分别识别出的三角形支架顶点的图像坐标，采用最小二乘法对立体投影模型进行计算，得到三角形支架顶点在世界坐标系中的3D坐标，并根据此三点的坐标构建目标坐标系，计算目标坐标系相对于世界坐标系的位置、姿态；8.根据权利要求3所述的空间机器人目标捕获自主路径规划，其特征在于所述的目标运动的预测步骤首先建立目标的运动的状态方程，然后采用扩展Kalman滤波器预测目标卫星下一个时刻的相对位置、姿态及相对线速度、角速度；9.根据权利要求3所述的空间机器人目标捕获自主路径规划，其特征在于所述的空间机器人末端运动速度规划步骤根据下式规划机械臂末端的运动速度上式中，，^、、"分别为规划的末端线速度和角速度；K为增益矩阵，为对角阵，用于限制规划的末端最大速度；E^、、、E條:分别为预测的目标下一个时刻的相对位置、线速度和角速度，在机械臂末端坐标系中的表示，AO为姿态指向误差，按下式计算<formula>formulaseeoriginaldocumentpage4</formula>其中，[rv，aj和[nh，oh，ah]分别为机械臂末端坐标系和目标坐标系的姿态变换矩阵，而^^为通过手眼相机测出的手柄相对于末端坐标系的姿态矩阵。10.根据权利要求3所述的空间机器人目标捕获自主路径规划，其特征在于所述的空间机器人避奇异的路径规划将空间机器人的动力学奇异回避转化为实时的运动学奇异回避，即<formula>formulaseeoriginaldocumentpage4</formula>(3)上式左边为机械臂末端相对于基座的运动速度(在基座坐标系中的表示)，因而可写为<formula>formulaseeoriginaldocumentpage4</formula>、°、￡<分别表示机械臂末端相对于基座的线速度、角速度在基座坐标系中的表示；￡/为机械臂的普通Jacobian矩阵，由此，根据(4)求解期望的关节角速度6j全文摘要空间机器人非合作目标自主识别与捕获方法包括基于立体视觉的位姿测量1、空间机器人目标捕获自主路径规划2、空间机器人系统协调控制3等主要步骤。基于立体视觉的位姿测量1对左、右相机图像进行实时处理，包括平滑滤波、边沿检测、直线提取等，计算非合作目标星相对于基座和末端的位姿；空间机器人目标捕获自主路径规划2，根据位姿测量结果，实时规划各关节的运动轨迹；空间机器人系统协调控制3则将机械臂的控制与基座的控制协调进行，实现整个系统的最佳控制性能。该方法不要求目标星上安装标志器或角反射镜，也无需知道目标的几何尺寸，而是直接以航天器自身的部件作为识别和捕获的对象，所规划的路径能有效回避动力学与运动学奇异点。文档编号B25J9/00GK101733746SQ20091007347公开日2010年6月16日申请日期2009年12月22日优先权日2009年12月22日发明者徐文福,李成,梁斌,王学谦,齐海萍申请人:哈尔滨工业大学