CN111640153B - 基于视觉与惯性单元融合的空间刚体质心位置检测方法 - Google Patents

Abstract

本发明为基于视觉与惯性单元融合的空间刚体质心位置检测方法。该检测方法在待检测空间刚体上布置多个带有光源的惯性单元，包括质心计算模块，以惯性单元光源所在位置h_k为球心，惯性单元光源所在位置到空间刚体质心的距离r_k为半径建立多个半径测量球O_k；通过最小二乘法相关原理，利用已经建立的多个半径测量球O_k，找出空间中一点Qⁱ距离各球面距离之和最短，并将该点坐标作为i时刻测量的质心点位置；不断进行多时刻质心位置计算，进行多时刻质心位置融合，不断修正质心位置，当两时刻质心位置误差小于5％时，视该质心位置为精确位置。方法针对空间刚体质心检测而设计，该方法将视觉和惯性单元融合，优势互补，提高了该方法的鲁棒性。

Description

基于视觉与惯性单元融合的空间刚体质心位置检测方法

技术领域

本发明涉及空间刚体质心测量技术领域，具体是一种用于测量空间刚体质心位置的检测方法。

背景技术

自上世纪50年代苏联发射了世界第一颗人造卫星“斯帕特尼克1号”，人类已经进行了数千次的航天器发射，地球周围各个轨道已经被数以百万计的空间刚体所覆盖，为了确定空间刚体的动力学数据，首先需要确定空间刚体的质心数据。空间刚体中除去数量较少、尺寸较大的在轨设备外，失效卫星、火箭末级以及碰撞衍生物等产生了数量庞大、尺寸不一的空间碎片，尤其是。2019年3月，印度进行了该国的第一次反卫星实验，其利用一枚三级导弹击中了印度军事卫星微星-R，并在近地轨道产生了数百个大小不一的碎片，更进一步增加了空间碎片数量，这就需要一种对合作目标和非合作目标具有普适性的空间刚体质心检测方法。

根据NASA和ESA的观察数据，空间刚体中非合作目标多为类圆柱形、类长方体的刚体，并且由于受到例如引力梯度及光压等各种摄动力的影响，其运动多为绕质心转动，在对其进行捕获时候需要首先知道其质心位置；而在轨航天器等合作目标更是依据其质心的动力学特性进行运动，同时在卫星消旋设备对空间碎片进行相应消旋冲击时，由于消旋机械臂的伸出，改变了卫星的质心位置，因此此时需要对卫星的质心进行判定以稳定卫星的运动，因此空间刚体的质心位置的确定是一种迫在眉睫的需求。

国防科技大学航天与材料工程学院的谢堂涛等人提出了一种基于视觉特征点的空间非合作目标质心识别方法，该方法通过视觉测量、匹配空间目标上的特征点，解算出特征点的速度信息和位置信息，再带入空间刚体速度方程中进行解算，在光照条件较好的情况下能够达到较好的精度，但是只采用视觉方法进行测量很容易受到空间杂光的干扰，在光照条件过亮或过暗的情况下，特征点的识别极为困难，因此该方法的适应性较差。

发明内容

针对现有对空间刚体质心位置检测技术的不足，本发明解决的技术问题是，提供一种基于视觉与惯性单元融合的空间刚体质心位置检测方法及检测装备。

本发明解决所述技术问题采用的技术方案是：

一种基于视觉与惯性单元融合的空间刚体质心位置检测方法，其特征在于，该检测方法在待检测空间刚体上布置多个带有光源的惯性单元，包括质心计算模块，以惯性单元光源所在位置h_k为球心，惯性单元光源所在位置到空间刚体质心的距离r_k为半径建立多个半径测量球O_k；

通过最小二乘法相关原理，利用已经建立的多个半径测量球O_k，找出空间中一点Qⁱ距离各球面距离之和最短，并将该点坐标作为i时刻测量的质心点位置；

不断进行多时刻质心位置计算，进行多时刻质心位置融合，不断修正质心位置，当两时刻质心位置误差小于5％时，视该质心位置为精确位置。

该方法的步骤是：

第一步、数据准备

将多惯性单元分散布置在空间刚体的合适位置，所述空间刚体为合作目标或者非合作目标，若空间刚体为合作目标则手动布置，若被检测空间刚体为非合作目标，则需要通过弹射方式将惯性单元发射到其表面，在接触到非合作目标表面后，惯性单元上的光源设备开启；

多个惯性单元在接触到空间刚体后，对空间刚体的相关参数进行采集，并获得滤波后空间刚体的多个惯性单元的相应惯性数据，其包括滤波后初始绕三轴的角速度

线加速度

第二步、数据测量

数据处理系统获得相应惯性数据后，确定各个惯性单元所处的空间距离信息，并针对不同惯性单元生成不同的惯性单元触发信号，保证同一个空间刚体上的多个惯性单元在同一时刻开始进行运动学参数采集，同时确定相应的惯性单元的视觉采集开始信号，保证视觉采集数据能够与惯性单元数据相对应；

调整多个惯性单元之间的触发时间，并在同一时刻开始采集，获取滤波后的惯性单元的当前时刻相应的运动学数据，其包括i时刻的三轴角速度

线加速度

和倾角

形成高频数据流P；

数据处理系统根据高频数据流P解算出i时刻k惯性单元光源所在位置到空间碎片质心的距离

双目视觉采集模块在惯性单元开始采集的同时进行图像数据采集，解算出空间刚体上惯性单元k的光源的三维坐标位置

第三步、质心计算

质心计算模块利用i时刻k惯性单元光源所在位置到空间碎片质心的距离

为半径，以惯性单元光源所在位置h_k ⁱ为球心建立多个半径测量球O_k；

再通过最小二乘法相关原理，利用已经建立的多个半径测量球O_k，找出i时刻空间中一点Qⁱ距离各球面距离之和最短，并将该点坐标作为i时刻测量的质心点位置；

第四步、质心位置融合

不断进行多个时刻质心位置计算，并通过贝叶斯方法进行多时刻质心位置融合，不断修正质心位置，当两时刻质心位置误差小于5％时，视该质心位置为精确位置；

第五步、当空间刚体发生碰撞或者其他改变质心空间位置的情况，重复第二步至第四步的过程，从而得到当前状态的空间刚体质心位置。

第三步中，确定i时刻空间中一点Qⁱ距离各球面距离之和最短的过程中，采用起始点变异的改进LM算法迭代计算获得i时刻的质心点位置，具体是：先通过LM算法内部先进行迭代，然后再进行LM算法循环，循环时以正负偏差0.5倍的初始点和最终迭代点之间的距离作为循环步长，以最终迭代点坐标正负偏差0.5倍的初始点和最终迭代点之间的距离获得两个新的起点，两个起点对应得到两个最终迭代点，并将计算后的这两个最终迭代点带入目标函数F中，得到最终函数值

和

比较函数值

与最终函数值

找出其中的最小值，再将最小值对应的质心位置初值和最终值作为下一次算法循环初始点和最终迭代点，用于进行下一次循环步长计算，当两个最终函数值与当前循环次数的函数值之间的差值小于1％，停止i时刻的LM算法循环，并将循环停止时循环终值坐标点作为i时刻的质心点位置。

第四步中，多时刻的质心点坐标按照公式

进行融合，其中O^g+1为融合后质心坐标；O^g为上一时刻的融合质心坐标，为该式在上一时刻的应用,如果为初始的两个时刻，则O^g则表示第一时刻由第三步的循环得到的质心坐标O¹；Oⁱ⁺¹为i+1时刻由第三步的循环得到的质心点坐标；μ^g表示的是融合质心坐标O^g的质心数据误差，μⁱ⁺¹为i+1时刻求出的质心数据误差，i＝g。

本发明还提供一种基于视觉与惯性单元融合的空间刚体质心位置的检测装备，包括多惯性单元测量系统、视觉定位系统、数据处理系统，

所述多惯性单元测量系统包括多个惯性单元、光源设备、滤波处理模块和信号传输模块；每个惯性单元上安装有一个光源设备，且同一个空间刚体上的不同惯性单元上的光源设备发光颜色不同，在惯性单元与空间刚体进行连接后自动开启；每个惯性单元内加载有滤波处理模块，并连接信号传输模块，信号传输模块与数据处理系统进行数据交换；

所述惯性单元获取滤波后空间刚体的加速度、角速度；

所述的信号传输模块能够在采集开始前接收信号处理系统发送的采集开始信号，给予惯性单元采集命令，确保各个惯性单元在同一时间采集和发送数据，并将滤波后的运动学数据以高频率发送给数据处理系统；

所述视觉定位系统包括双目视觉采集模块、图像处理模块；

所述双目视觉采集模块，通过对空间刚体表面光源设备拍照，获取到同一时刻不同距离相机下的图像，为惯性单元光源所在点的三维坐标解算提供数据支持；

图像处理模块，通过对同一时刻不同位置的两个相机拍摄的图片进行去畸变，比对不同图片中不同颜色发光点的像素差，利用三角测量的方法解算出空间碎片上惯性单元光源所在点的三维坐标；

所述数据处理系统包括触发信号处理模块、质心计算模块和数据传输模块；

所述质心计算模块对滤波后的惯性数据进行质心计算，以惯性单元光源所在位置h_k为球心，惯性单元光源所在位置到空间刚体质心的距离r_k为半径建立多个半径测量球O_k，最后通过最小二乘法计算出空间刚体的质心数据。

与现有技术相比，本发明有益效果在于：

(1)利用惯性单元直接获取惯性参数，比起非接触式测量方式如视觉等，测量的相关惯性数据的准确程度更高，能够保证后期其质心位置的计算更为精确、可靠。

(2)在得到惯性单元位置与质心之间的粗略距离参数后，建立多质心测量球体，并通过最小二乘法找出空间中与各球面距离最近点作为质心位置，该方式本质上又进行了一次惯性数据滤波与融合，进一步提高了计算数据的准确性，同时利用最小二乘法可以借助优化设计的相关知识，找出其中最优点作为质心位置，保证了该方法在数学和物理上的可靠性。

(3)本发明检测方法针对空间刚体质心检测而设计，该方法将视觉和惯性单元融合，能够充分发挥视觉传感器和惯性单元的优点，在视觉测量误差较大的动力学数据方面采用较为准确的惯性单元进行测量，其测量的精度高、数据连续，规避掉了视觉传感器对光敏感、视觉测量时存在的较大截断误差的缺点，而惯性单元无法做到的空间位置测量由视觉传感器完成，从而将两种传感器进行融合优势互补，提高了该方法的鲁棒性。同时在每个惯性单元上设置不同颜色的光源设备，能给视觉传感器提供可识别点，让视觉传感器在不同光照条件下均可以进行测量。

(3)利用扩展卡尔曼滤波进行多惯性单元之间的初始数据噪声消除，保证了初始数据的准确性，为后续解算空间碎片上惯性单位的距离信息提供了较为良好的源数据。

(4)该方法可以保证在空间刚体质心位置发生变化时候，通过该方法快速计算出质心位置，从而方便在轨航天器快速进行配重以稳定其姿态，或是在空间碎片消旋过程中通过该位置快速计算出其他相关惯性参数，提高消旋的效率。

(5)该方法使用范围较广，基本上适用于所有空间中的在轨设备或其他刚性较大的空间物质，且其快速性也保证了当空间刚体存在较大动能时候也能即时、快速的识别出质心位置。

综上，本发明为解决当前空间刚体质心检测方法中，视觉方法检测质心的精度较差、效率较低、应用条件苛刻、实时性较差，以及难以应用到实际的复杂太空光照环境中等问题而提出，本申请方法能够较好的解决上述问题，其采用惯性单元和视觉传感器相结合的方式，弥补了视觉传感器光敏感的缺点，能够在不同的光照环境下均取得2％～5％的质心位置测量精度。

附图说明

图1为空间碎片上惯性单元的安装示意图；

图2为视觉定位系统在检测设备上的安装示意图；

图3为双目视觉采集模块测量惯性单元光源所在位置的示意图，基于视觉测量的原理，即视觉测量惯性单元的光源点，利用双目视觉的两幅图像差异解算光源点的空间位置；

图4为空间刚体质心检测原理简图，该图展示了质心测量的原理，即利用视觉测量的惯性单元光源点位置，和利用运动方程解算出来的光源点到质心的距离，建立起多个球体，通过最小二乘法找出空间一点到多个球体表面最近的一点，完成对质心位置的检测；

图5为空间刚体质心检测方法流程图，包括多惯性单元数据速度获取、视觉位置数据获取、质心检测球体的建立以及最小二乘法解算质心位置；

图6为仅使用现有视觉定位获得的质心计算误差数据；

图7为本发明检测方法获得的质心计算误差数据；

图8为LM算法流程示意图。

具体实施方式

下面给出本发明的具体实施例。具体实施例仅用于进一步详细说明本发明，不限制本申请权利要求的保护范围。

本发明针对空间碎片的主动移除过程中，空间碎片等非合作目标的质心变化的识别，以及消旋装置卫星等合作目标由于机械臂伸出等过程造成的卫星质心变化识别提出一种空间刚体质心位置的检测方法，其能够保证移除装置对空间碎片消旋点选取的准确性，以及消旋装置展开后消旋设备的稳定性，保证消旋过程中移除装置的正确执行与空间碎片质心的精确识别。

本发明提供了一种基于视觉与惯性单元融合的空间刚体质心位置检测装备包括多惯性单元测量系统、视觉定位系统3、数据处理系统三部分；所述视觉定位系统位于检测设备5前端，多惯性单元测量系统在测量前已经通过发射或者手动布置放置到了空间刚体1上，数据处理系统位于检测设备5的内部；检测设备安装在航天器上。

所述多惯性单元测量系统包括多个惯性单元2、光源设备4、滤波处理模块和信号传输模块。每个惯性单元上安装有一个光源设备，且同一个空间刚体上的不同惯性单元上的光源设备发光颜色不同；每个惯性单元内加载有滤波处理模块，并连接信号传输模块，信号传输模块与数据处理系统进行数据交换；

所述惯性单元2是获取数据的主体，其由陀螺仪、加速度计和倾角计等三种传感器组成，通过与空间刚体接触，惯性单元2能较为准确的获取到空间刚体的(包括碎片和航天器)加速度、角速度、偏航角等运动学参数，方便数据的后续处理；所述光源设备4位于惯性单元的表面，在惯性单元2与空间碎片1进行连接后自动开启，保证视觉定位系统能够在太空中较差的光照条件下对惯性单元2位置进行识别，同时不同惯性单元的光源设备的发光颜色不同，提高了视觉系统识别的正确率；

所述滤波处理模块位于惯性单元2内部，其在惯性单元采集数据后，利用卡尔曼滤波对采集的运动学数据进行滤波，提高惯性单元2测量数据的准确性，为后续质心位置解算奠定基础；

所述的信号传输模块位于惯性单元内部，其能够在采集开始前接收数据处理系统的触发信号处理模块发送的采集开始信号，给予惯性单元2采集命令，确保各个惯性单元在同一时间采集和发送数据，并将滤波后的运动学数据以高频率发送给数据处理系统，保证了发送信号准确性。

所述视觉定位系统包括双目视觉采集模块3、图像处理模块；

所述双目视觉采集模块3安装在检测设备5的前端，调整两相机相距以提供足够的视差和视野范围，通过对空间碎片1表面光源设备拍照，获取到同一时刻不同距离相机下的图像，为惯性单元2所在点的三维坐标解算提供数据支持；

所述图像处理模块位于检测设备5内部，该系统通过对同一时刻不同位置的两个相机拍摄的图片进行去畸变，提高三维点坐标识别的正确率，之后对去畸变后的坐标进行解算，通过比对不同图片中不同颜色发光点的像素差，利用三角测量的方法解算出空间碎片1上惯性单元2所在点的三维坐标。

所述数据处理系统包括触发信号处理模块、质心计算模块和数据传输模块。

所述触发信号处理模块，通过接收各惯性单元2发送的滤波后惯性数据，利用电磁波速率计算公式，获取各个惯性单元2所处的距离信息，针对对不同距离的惯性单元生成并发送采集开始信号(惯性单元触发信号)，保证位于同一空间刚体上的各惯性单元2在同一时刻开始进行数据采集；

所述质心计算模块，其能够对滤波后的惯性数据进行相关计算，其根据空间三维物体运动学公式，利用空间刚体的角速度和速度数据计算出各惯性单元光源所在位置距质心的半径数据，再根据视觉采集到的各惯性单元所在点的三维坐标，以该点为球心，以这些半径数据做球的半径画出相应空间球，最后通过最小二乘法计算出空间刚体的质心数据；

所述数据传输模块，其在采集开始前发送惯性单元采集触发信号和视觉采集开始信号，并在采集后用于接收视觉定位系统和多惯性单元测量系统的相应数据，同时能够识别不同惯性单元2发送数据的校验位。

惯性单元可以为HY-300惯性传感器，双目视觉采集模块为Prosilica GT6400视觉相机。本申请中的检测设备可以为空间合作目标，如航天器等。

本发明提供的基于视觉与惯性单元融合的空间刚体质心位置检测方法，该实例方法的实施步骤为：

步骤一、将多惯性单元2分散布置在空间刚体的不同位置，视觉传感器范围内能同时出现两个或以上的惯性传感器。若空间刚体为合作目标则无需其他操作，手动将各个惯性单元布置在合作目标上，惯性单元的光源设备4开启；若被检测空间刚体为非合作目标，则需要通过弹射方式将惯性单元发射到其表面，在接触到空间碎片表面后，惯性单元的光源设备4开启，保证了视觉定位系统在较差的光源条件下也能较为精确的识别惯性单元位置；

步骤二、多惯性单元2在接触到空间碎片后，首先惯性单元通过接触式测量方式，对空间刚体的相关参数进行采集，其利用其自身携带的三轴陀螺仪、三自由度加速度计，得到空间刚体的相应惯性数据，其包括初始绕三轴的角速度ω⁰、线加速度α⁰，然后滤波处理模块通过卡尔曼滤波滤除掉初始数据的噪声，再通过调频后得到初始数据

线加速度

最后通过信号传输模块将滤波后数据

传递至该检测设备中的数据处理系统；

步骤三、在数据处理系统接收各惯性单元的信号传输模块发送的数据

触发信号处理模块工作，首先通过解调恢复初始数据原状，再通过识别解调后信号的校验位，识别出不同惯性单元发送的步骤二获得的滤波数据，然后利用电磁波速率计算公式，

L＝ct，c为速度，电磁波速度，已知常数；t为时间，惯性单元的采样间隔，由测量得到；

获取到惯性单元的空间距离信息，在获取到该距离信息后，针对不同惯性单元生成不同的惯性单元触发信号，确定各个惯性单元的采集开始时间，保证同一个空间刚体上的多个惯性单元在同一时刻开始进行运动学参数采集；同时该系统会通过各个惯性单元的采集开始时间生成对应惯性单元的视觉采集开始信号，保证视觉采集数据能够与惯性单元数据二者同时采集，且数据相对应，最后通过数据处理系统中的数据传输模块，将不同的惯性单元触发信号(惯性开始信号)发送至不同的惯性单元；

步骤四、多个惯性单元的信号传输模块在接收到触发信号后，利用该信号调整多个惯性单元之间的触发时间，并在同一时刻开始采集，惯性单元2利用其自身携带的陀螺仪、加速度计和倾角计进行相应的运动学数据采集，其包括i时刻的三轴角速度ωⁱ、线加速度αⁱ，然后滤波处理模块通过卡尔曼滤波滤除采集数据的噪声Uⁱ和Wⁱ，将数据变为滤波后数据

其中字母下的下角标xyz分别对应惯性单元的XYZ轴上对应参数的投影。

最后将滤波后的数据变为高频数据流P，通过信号传输模块发送给数据处理系统；

步骤五、数据处理系统利用数据传输模块接收到到高频数据流P后，首先将该数据流进行解调还原为原有数据，再根据数据的校验位确定该数据的数据源对应的惯性单元，再通过两帧间的角速度，求出其角加速度

之后根据定点刚体自由旋转运动公式，

解算出i时刻k惯性单元光源所在位置到空间碎片质心的距离

其中

为上述的定点刚体自由旋转运动公式所求，其中i代表其为i时刻所求；

步骤六、双目视觉采集模块3在惯性单元2采集开始时会接收到数据处理系统中触发信号处理模块发送的视觉采集开始信号，保证双目视觉采集模块3在惯性单元2开始采集的同时进行图像数据采集，该采集速率为20fps，在采集过后将该图像数据导入视觉定位系统的图像处理模块中；

步骤七、图像处理模块在接收到双目视觉采集模块发送来的图像数据后，首先将图像进行去畸变处理，通过在采集开始前的标定获取到的去畸变系数k₁、k₂、k₃、p₁、p₂，对获取图像进行去畸变处理，保证获取到的惯性单元位置更为准确；

步骤八、在进行去畸变后，图像处理模块对修正后的图片进行三角测量处理，通过识别惯性单元上的光源设备4获得惯性单元在图像上的投影，再利用双目视觉采集模块3获取到的图像投影关系、像素位置关系以及相机之间的距离b，解算出空间碎片上惯性单元k的光源的三维坐标位置h_k ⁱ，其公式如下，

f为焦距，u为图像坐标系中的横坐标，v为图像坐标系中的纵坐标，b代表的视差，即为实际两相机间的距离。

步骤九、质心计算模块利用步骤五求得的i时刻k惯性单元光源所在位置到空间碎片质心的距离

以惯性单元2光源所在位置h_k ⁱ(X_k ⁱ、Y_k ⁱ、Z_k ⁱ)为球心，以距离

为半径建立多个半径测量球O_k，

其中，k为所测量的惯性单元，其最大值与布置在空间刚体上的数量有关，本实施例中k的最大值为3；i是当前时刻i。

然后质心计算模块通过最小二乘法相关原理，利用已经建立的多个半径测量球O_k，找出i时刻空间中一点Qⁱ(x0、y0、z0)距离各球面距离之和最短，

并利用LM算法迭代出该时刻的质心点位置，设置其公式如下，

在本算法中取初值点为

LM算法的公式中，

和

分别代表第n+1次迭代的质心点坐标和第n次迭代的质心点坐标；λ为阻尼因子，通过计算其来调整在不同迭代步数时算法的下降速度和精度，同时能够保证系数矩阵正定，从而确保迭代的下降方向；J为雅可比矩阵；F为目标函数。

步骤十、由于LM算法在迭代计算时，容易陷入局部最小值点，本发明采用起始点变异的改进多次迭代算法，首先当步骤九的迭代结束后，获得步骤九的最终迭代点为

该最终迭代点为迭代l次获得，将最终迭代点

带入目标函数F中，并将

计算出的函数值存为

然后计算新的循环步长，假设该次循环的初始点为

则首先需要计算初始点和最终迭代点之间的距离，

再利用该距离按照最终迭代点坐标正负偏差0.5倍的初始点和最终迭代点之间的距离，计算出两个新的起点，

再然后利用这两个起点进行LM迭代，两个起点对应得到两个最终迭代点

和

并将计算后的这两个最终迭代点带入目标函数F中，得到最终函数值

和

比较函数值

与最终函数值

找出其中的最小值，再将最小值对应的质心位置初值和最终值作为下一次算法循环初始点

和最终迭代点

用于进行下一次循环步长计算，l+1表示的是算法l+1次循环，当重复上述步骤十的循环步骤m次后，当

之间的差值小于1％，停止i时刻的算法循环，并将

作为i时刻的质心计算值；

步骤十一、不断进行多个时刻的质心迭代循环测量，并通过贝叶斯方法进行多时刻质心位置融合，g表示融合时刻的数据，

其中O^g+1为融合后质心坐标；O^g为上一时刻的融合质心坐标，其为上式在上一时刻的应用,如果为初始的两个时刻，则O^g则表示第一时刻由步骤十的循环得到的质心坐标O¹；Oⁱ⁺¹为i+1时刻由步骤十的循环得到的质心点坐标；μ^g表示的是融合质心坐标O^g的质心数据误差，其由融合该数据的所有循环初值和循环终值的平均差值决定，μⁱ⁺¹为i+1时刻求出的质心数据误差，其由i+1时刻循环停止时对应的循环次数的循环初值

以及循环终值

决定，μ^g和μⁱ⁺¹其计算方式为，i和g在数值上相等，

n为融合质心点数量，μ^x的求法见下式

不断修正质心位置，当两时刻质心位置误差小于5％时，可以视该质心位置为精确位置；

步骤十二、当空间刚体发生碰撞或者其他改变质心空间位置的情况，重复以上测量、融合、计算的步骤，从而得到当前状态的空间碎片质心位置，使质心位置检测更加准确，满足实时性要求。

为了验证本申请检测方法的准确性，以背景文献中的单纯由视觉数据测量出来的质心位置方法和本申请检测方法进行实验对比，结果如图6和图7所示，图6中为光照条件较好下，随着时间的推移，质心计算误差稳定在一个比较大的范围内(4.2％～5％)，若在光照条件不好的情况下，质心计算误差稳定范围更大；而在本申请使用两种传感器结合的方法后，无论是在光照条件好还是差的情况下，其质心计算误差最终都能稳定于一个较小的区间范围(3.2％～4％)，说明了本申请惯性传感器和视觉传感器融合的质心检测方法能够大大减小质心位置测量误差。

本发明对于空间刚体为航天器时，其检测方法步骤同上述的过程。

本发明未述及之处适用于现有技术。

Claims (5)

1.一种基于视觉与惯性单元融合的空间刚体质心位置检测方法，其特征在于，该检测方法在待检测空间刚体上布置多个带有光源的惯性单元，包括质心计算模块，以i时刻k惯性单元光源所在位置h_k ⁱ为球心，i时刻k惯性单元光源所在位置到空间刚体质心的距离

为半径建立多个半径测量球O_k；i时刻k惯性单元光源所在位置到空间刚体质心的距离

由i时刻k惯性单元的角速度根据定点刚体自由旋转运动公式解算获得；

通过最小二乘法原理，利用已经建立的多个半径测量球O_k，找出空间中一点Qⁱ距离各球面距离之和最短，并将该点坐标作为i时刻测量的质心点位置；

不断进行多时刻质心位置计算，进行多时刻质心位置融合，不断修正质心位置，当两时刻质心位置误差小于5％时，视该质心位置为精确位置。

2.根据权利要求1所述的检测方法，其特征在于：该方法的步骤是：

第一步、数据准备

将多惯性单元分散布置在空间刚体的合适位置，所述空间刚体为合作目标或者非合作目标，若空间刚体为合作目标则手动布置，若被检测空间刚体为非合作目标，则需要通过弹射方式将惯性单元发射到其表面，在接触到非合作目标表面后，惯性单元上的光源设备开启；

多个惯性单元在接触到空间刚体后，对空间刚体的相关参数进行采集，并获得滤波后空间刚体的多个惯性单元的相应惯性数据，其包括滤波后初始绕三轴的角速度

线加速度

第二步、数据测量

数据处理系统获得相应惯性数据后，确定各个惯性单元所处的空间距离信息，并针对不同惯性单元生成不同的惯性单元触发信号，保证同一个空间刚体上的多个惯性单元在同一时刻开始进行运动学参数采集，同时确定相应的惯性单元的视觉采集开始信号，保证视觉采集数据能够与惯性单元数据相对应；

调整多个惯性单元之间的触发时间，并在同一时刻开始采集，获取滤波后的惯性单元的当前时刻相应的运动学数据，其包括i时刻的三轴角速度

线加速度

和倾角

形成高频数据流P；

数据处理系统根据高频数据流P解算出i时刻k惯性单元光源所在位置到空间刚体 质心的距离

双目视觉采集模块在惯性单元开始采集的同时进行图像数据采集，解算出空间刚体上惯性单元k的光源的三维坐标位置h_k ⁱ；

第三步、质心计算

质心计算模块利用i时刻k惯性单元光源所在位置到空间刚体 质心的距离

为半径，以惯性单元光源所在位置h_k ⁱ为球心建立多个半径测量球O_k；

再通过最小二乘法原理，利用已经建立的多个半径测量球O_k，找出i时刻空间中一点Qⁱ距离各球面距离之和最短，并将该点坐标作为i时刻测量的质心点位置；

第四步、质心位置融合

不断进行多个时刻质心位置计算，并通过贝叶斯方法进行多时刻质心位置融合，不断修正质心位置，当两时刻质心位置误差小于5％时，视该质心位置为精确位置；

第五步、当空间刚体发生碰撞或者其他改变质心空间位置的情况，重复第二步至第四步的过程，从而得到当前状态的空间刚体质心位置。

3.根据权利要求2所述的检测方法，其特征在于，第三步中，确定i时刻空间中一点Qⁱ距离各球面距离之和最短的过程中，采用起始点变异的改进LM算法迭代计算获得i时刻的质心点位置，具体是：先通过LM算法内部先进行迭代获得最终迭代点，然后再进行LM算法循环，循环时以正负偏差0.5倍的初始点和最终迭代点之间的距离作为循环步长，以最终迭代点坐标正负偏差0.5倍的初始点和最终迭代点之间的距离获得两个新的起点，两个起点对应得到两个最终迭代点，并将计算后的这两个最终迭代点带入目标函数F中，得到最终函数值

和

比较函数值

与最终函数值

找出其中的最小值，再将最小值对应的质心位置初值和最终值作为下一次算法循环初始点和最终迭代点，用于进行下一次循环步长计算，当两个最终函数值与当前循环次数的函数值之间的差值小于1％，停止i时刻的LM算法循环，并将循环停止时循环终值坐标点作为i时刻的质心点位置；

其中，目标函数F为利用已经建立的多个半径测量球O_k，找出空间中一点Qⁱ距离各球面距离之和最短；通过LM算法内部迭代l次获得最终迭代点，将最终迭代点带入目标函数F中获得

4.根据权利要求3所述的检测方法，其特征在于，第四步中，多时刻的质心点坐标按照公式

进行融合，其中O^g+1为融合后质心坐标；O^g为上一时刻的融合质心坐标，如果为初始的两个时刻，则O^g则表示第一时刻由第三步的循环得到的质心坐标O¹；Oⁱ⁺¹为i+1时刻由第三步的循环得到的质心点坐标；μ^g表示的是融合质心坐标O^g的质心数据误差，μⁱ⁺¹为i+1时刻求出的质心数据误差，i＝g。

5.一种基于视觉与惯性单元融合的空间刚体质心位置的检测装备，包括多惯性单元测量系统、视觉定位系统、数据处理系统，其特征在于：

所述多惯性单元测量系统包括多个惯性单元、光源设备、滤波处理模块和信号传输模块；每个惯性单元上安装有一个光源设备，且同一个空间刚体上的不同惯性单元上的光源设备发光颜色不同，在惯性单元与空间刚体进行连接后自动开启；每个惯性单元内加载有滤波处理模块，并连接信号传输模块，信号传输模块与数据处理系统进行数据交换；

所述惯性单元获取滤波后空间刚体的加速度、角速度；

所述的信号传输模块能够在采集开始前接收信号处理系统发送的采集开始信号，给予惯性单元采集命令，确保各个惯性单元在同一时间采集和发送数据，并将滤波后的运动学数据以高频率发送给数据处理系统；

所述视觉定位系统包括双目视觉采集模块、图像处理模块；

所述双目视觉采集模块，通过对空间刚体表面光源设备拍照，获取到同一时刻不同距离相机下的图像，为惯性单元光源所在点的三维坐标解算提供数据支持；

图像处理模块，通过对同一时刻不同位置的两个相机拍摄的图片进行去畸变，比对不同图片中不同颜色发光点的像素差，利用三角测量的方法解算出空间刚体 上惯性单元光源所在点的三维坐标；

所述数据处理系统包括触发信号处理模块、质心计算模块和数据传输模块；

所述质心计算模块对滤波后的惯性数据进行质心计算，以i时刻k惯性单元光源所在位置h_k ⁱ为球心，i时刻k惯性单元光源所在位置到空间刚体质心的距离

为半径建立多个半径测量球O_k，最后通过最小二乘法原理计算出空间刚体的质心数据。

Images