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

权利要求书

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％时，视该质心位置为精确位置；

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