CN108734737B - 基于视觉slam估计空间旋转非合作目标转轴的方法 - Google Patents

Abstract

基于视觉SLAM估计空间旋转非合作目标转轴的方法，本发明涉及估计空间旋转非合作目标转轴的方法。本发明的目的是为了解决现有方法估计的精度较差，计算条件受限制，以及并未考虑到相机拍摄的图片中存在其他碎片等干扰，使旋转非合作目标转轴估计的误差大的问题。过程为：一、得到训练好的YOLO目标检测模型；二、建立k叉数字典；三、估计出每帧旋转矩阵和平移矩阵；四、对RGBD相机采集的每帧图像处理，直至检测出两次回环或达到指定时间或帧数，RGBD相机停止采集图像；五、拟合成的空间平面的法线是非合作目标转轴的斜率，空间圆弧的圆心是非合作目标转轴经过的点。本发明用于航天在轨服务领域。

Description

基于视觉SLAM估计空间旋转非合作目标转轴的方法

技术领域

本发明涉及估计空间旋转非合作目标转轴的方法。本技术应用于航天在轨服务领域。

背景技术

目前，世界各国发射了各式各样的航天飞行器，如卫星、航天飞机、空间站以及深空探测器等用来探索和开发太空。在全球每年发射的100多颗卫星里，会有2-3颗未正常入轨，而正确入轨的卫星中，又有5颗左右在寿命初期即失效，失效的卫星还会产生大量的碎片和太空垃圾。因此，维修或清除空间中的故障或废弃的卫星具有巨大的社会经济效益和良好的应用前景。与空间站、三轴稳定卫星等运动状态已知或能提供运动信息和结构信息的合作目标不同，故障或废弃卫星在空气摄动力的作用下随着能量耗散可绕本体最大惯量轴旋转，其三维形状、尺寸大小未知，位置姿态、转轴、转动惯量不确定，且无法提供用于辅助测量的人工标记，这样的卫星称为空间旋转非合作目标。与空间三轴稳定的非合作目标不同，对旋转非合作目标进行在轨操作时必须要估计出目标的转轴位置，这给空间机器人的在轨服务带来巨大挑战。例如“哥伦比亚”号航天飞机曾试图对一颗处于自旋状态的非合作目标进行捕获，但未成功。

随着多传感器技术应用于机器人技术中，机器人变得越来越智能化，由仿生学可知，在人类的感官中，大脑得到的83％的信息量来自于视觉。相比于其它的感知方式，视觉有两个明显的优势，第一是获得的信息量较大，第二是非接触式测量。所以它被认为是卫星在轨服务最重要的感知方式。

对于旋转非合作目标转轴估计，国外学者做了大量的研究。

加拿大航天局(CSA)的Aghili提出了基于EKF的运动框架来对空间旋转非合作目标进行转轴估计，他成功地估计出了相机坐标系下非合作目标的位置和惯性主轴。但是Aghili针对的目标，旋转速度小于0.1rad/s，对于转速较大时的非合作目标转轴估计的精度较差；麻省理工大学的Brent Edward Tweddle在其博士生论文中，提出将非合作目标的动力学方程与SLAM结合，再用iSAM进行后端优化来估算非合作目标的转轴。他将非合作目标的动力学方程作为约束，并且假设无外力和外力矩。此方法虽然较好得估计出转轴位置，但是由于动力学方程的限制，无法将此算法扩展到其他不满足此动力学方程的场景，计算条件受限制。

此外，以上的转轴估计均没有对非合作目标进行识别与检测，均假设视野中仅存在非合作目标，并未考虑到相机拍摄的图片中存在其他空间碎片。事实上，后者出现的几率很大，在算法中如果不识别并截取目标，干扰中的特征点会大大影响算法的精度，使旋转非合作目标转轴估计的误差大。

发明内容

本发明的目的是为了解决现有方法对于转速较大时的非合作目标转轴估计的精度较差，计算条件受限制，以及现有转轴估计均没有对非合作目标进行识别与检测，均假设视野中仅存在非合作目标，并未考虑到相机拍摄的图片中存在其他碎片等干扰，使旋转非合作目标转轴估计的误差大的问题，而提出基于视觉SLAM估计空间旋转非合作目标转轴的方法。

基于视觉SLAM估计空间旋转非合作目标转轴的方法具体过程为：

步骤一、利用ImageNet数据库对YOLO目标检测模型做预训练；将前期获得的非合作目标图片输入预训练好的YOLO目标检测模型进行训练，得到训练好的YOLO目标检测模型；

步骤二、对前期获得的非合作目标图片信息提取ORB特征点，利用K均值算法进行聚类建立k叉数字典；

步骤三、设三个坐标系，分别是C坐标系、C0坐标系、B坐标系；

C坐标系为相机坐标系；B坐标系为非合作目标本体坐标系，且过非合作目标质心；C0坐标系与B坐标系运动同步，且相机采集到第一帧置信值大于0.95的图像时，C0坐标系与C坐标系重合；

各坐标系的实际运动情况为：以地面为参考系C坐标系静止；B坐标系以恒定角速度自旋；C0坐标系的运动与B坐标系转动同步，随时间移动，C0坐标系原点的轨迹呈圆弧；

利用相对运动原理，将各坐标系的实际运动情况转换成SLAM算法适用的场景：B坐标系与C0坐标系静止，C坐标系绕B坐标系转动；估计出每帧C坐标系相对于C0坐标系的旋转矩阵Rk和平移矩阵tk；具体过程为：

步骤三一、RGBD相机采集彩色图像和深度图像，将RGBD相机采集的彩色图像输入训练好的YOLO目标检测模型进行实时目标识别与检测，若检测到某帧彩色图像的confidence值大于0.95，则停止YOLO模型检测，并记录YOLO输出的此帧彩色图像的x′，y′，w′，h值，将x′，y′，w′，h值构成的区域记作ROI区域；

所述confidence为置信度，x′，y′为目标所在图像像素区域的中心，w′为目标所在图像像素区域的宽，h为目标所在图像像素区域的高；它其实是一个比例，还有那个xywh它是个比例，比如图片是60*60，中心坐标是30*30，那么这个x′，y′就是0.5/0.5；

步骤三二、若RGBD相机采集到的彩色图像是confidence值大于0.95后的第i帧图像，i＝1；

提取彩色图像的confidence值大于0.95后的第i帧图像ROI区域中的ORB特征点，以C0坐标系为世界坐标系，根据深度图像的信息将ORB特征点转换为世界坐标系下的三维地图点，放入局部地图中，并将彩色图像的confidence值大于0.95后的第一帧设为关键帧；

所述第i帧图像为彩色图像的confidence值大于0.95后的第i帧图像；

关键帧的选取原则：距上一关键帧插入已经超过20帧，或者当前帧的ORB特征点与地图点成功匹配上了至少50个点；

步骤三三、若RGBD相机采集到的彩色图像是confidence值大于0.95后的第i帧图像，i≠1；

提取彩色图像的confidence值大于0.95后的第i帧图像ROI区域中的ORB特征点；

首先，局部地图中的地图点与彩色图像的confidence值大于0.95后的第i帧图像的ORB特征点进行匹配，并更新局部地图：

1)给定一个阈值，当匹配上的点大于阈值，删除局部地图中不在当前帧相机视野内的地图点；

2)当匹配上的点小于等于阈值时，将当前帧的所有的ORB特征点转换成地图点，放入到局部地图中；

3)对于关键帧，要将关键帧中所有的ORB特征点转换为地图点存入局部地图；

然后，根据匹配好的地图点与第i帧图像的ORB特征点，采用3D-2D:PnP方法，计算出第i帧图像的R_i和t_i，并将第1至第i帧的R_k和t_k传入步骤三四的图优化中进行后端优化；

步骤三四、建立因子图，用g2o对因子图进行后端优化，即对第1至第i帧图像的C坐标系相对于C₀坐标系的旋转矩阵R_k、平移矩阵t_k和局部地图上的地图点进行优化，得到优化后的旋转矩阵R_k、平移矩阵t_k和局部地图上的地图点，k＝1,2,…,i；

步骤三五、若检测到的彩色图像的confidence值大于0.95后的第i帧为被检测帧，则基于词袋模型进行回环检测，判断第i帧是否能检测出回环，若是，再次优化第1至第i帧图像的C坐标系相对于C₀坐标系的旋转矩阵R_k、平移矩阵t_k和局部地图上的地图点；若否，则执行步骤四；

若第i帧不是被检测帧，则执行步骤四；

步骤四、对YOLO目标检测模型输出的彩色图像的confidence值大于0.95后RGBD相机采集的每帧图像，均执行一次步骤三二至步骤三五，直至检测出两次回环或达到指定时间或帧数，RGBD相机停止采集图像；

步骤五、C坐标系相对C₀坐标系的运动轨迹为圆弧，设C₀坐标系的原点为p(0,0,0)，根据步骤四中估计出的每帧C坐标系相对于C₀坐标系的平移矩阵t_k，设每帧C坐标系的原点在C₀坐标系下为t_cc0，t_cc0＝p+t_k；

彩色图像的confidence值大于0.95后的n帧图像C坐标系在C₀坐标系下的原点三维坐标拟合成空间平面和空间圆弧；

所述n帧为RGBD相机停止采集图像前彩色图像的confidence值大于0.95后的所有帧；

拟合成的空间平面的法线是非合作目标转轴的斜率，空间圆弧的圆心是非合作目标转轴经过的点。

本发明的有益效果为：

本发明主要针对相机和非合作目标相对静止的场景，本发明基于深度学习的目标检测技术可对非合作目标进行识别和检测，有效减少相机视野中其他碎片的干扰，使转轴测量结果更加精确；解决了现有方法并未考虑到相机拍摄的图片中存在其他碎片等干扰，使旋转非合作目标转轴估计的误差大的问题；

本发明基于视觉SLAM技术仅靠图像间的匹配来确定相机位姿，没有利用动力学方程，从而对转轴测量的场景不受限制；解决了现有方法不满足此动力学方程的场景，计算条件受限制的问题；

本发明将基于深度学习的目标检测技术以及视觉SLAM技术相融合，来准确地对非合作目标的转轴定位，适用于所有转速，提高了对非合作目标转轴估计的精度；解决了现有方法对于转速较大时的非合作目标转轴估计的精度较差的问题。

根据图15和图16测得的转轴斜率：x方向误差在3％，y方向误差4％，z方向误差8％；测得转轴过经过的坐标点：x值误差在5％，y值误差在8％，z值误差在3.7％。

附图说明

图1为k叉数字典示意图，k为k叉数每层的节点数，d为k叉数的深度；

图2为算法中各坐标系的运动示意图；

图3为坐标系实际运动示意图；

图4为空间圆弧求圆心示意图，n为平面α的法向量；

图5为本发明实验场景示意图；

图6为本发明转轴斜率在各方向的测量值与真实值对比图；

图7a为前50帧C坐标系原点三维坐标图；

图7b为前50帧C坐标系原点球面拟合图；

图8a为前100帧C坐标系原点三维坐标图；

图8b为前100帧C坐标系原点球面拟合图；

图9a为前150帧C坐标系原点三维坐标图；

图9b为前150帧C坐标系原点球面拟合图；

图10a为前200帧C坐标系原点三维坐标图；

图10b为前200帧C坐标系原点球面拟合图；

图11为前50帧拟合出的空间球面与平面拟合图；

图12为前100帧拟合出的空间球面与平面拟合图；

图13为前150帧拟合出的空间球面与平面拟合图；

图14为前200帧拟合出的空间球面与平面拟合图；

图15为算法测出的非合作目标转轴经过的点的坐标值(空间圆弧圆心)与真值对比图；

图16为算法测出的非合作目标转轴经过的点的坐标值(空间圆弧圆心)与真值的误差图，单位为cm。

具体实施方式

具体实施方式一：本实施方式基于视觉SLAM估计空间旋转非合作目标转轴的方法具体过程为：

步骤一、利用ImageNet数据库对YOLO目标检测模型做预训练；将前期获得的非合作目标图片输入预训练好的YOLO目标检测模型进行训练，得到训练好的YOLO目标检测模型；

步骤二、对前期获得的非合作目标图片信息提取ORB特征点，利用K均值算法进行聚类建立k叉数字典；

具体过程为：

利用前期获得的非合作目标转轴建立字典，为后续的回环检测做准备；

建立字典的过程为：

首先对前期获得的非合作目标图片信息提取ORB特征点，利用K均值算法对ORB特征点进行聚类，将聚类后的结果用k叉树表示，如图1，在k叉树的根节点，用K均值算法把所有ORB特征点聚成k类，对k叉树第一层的每个节点，把属于该节点的ORB特征点聚成k类，得到下一层的每个节点；以此类推，最后得到叶子层的每个节点，叶子层的每个节点即为字典中的“Words”(单词)；

本实施方式中的k叉数的中间节点便于快速查找。

步骤三、设三个坐标系，分别是C坐标系、C0坐标系、B坐标系，如图2所示。

C坐标系为相机坐标系；B坐标系为非合作目标本体坐标系，且过非合作目标质心；C0坐标系与B坐标系运动同步，且相机采集到第一帧置信值大于0.95的图像时，C0坐标系与C坐标系重合，它会随非合作目标的自旋而转动；

各坐标系的实际运动情况为如图3：以地面为参考系C坐标系静止；B坐标系以恒定角速度自旋；C0坐标系的运动与B坐标系转动同步，随时间移动，C0坐标系原点的轨迹呈圆弧；

而在本算法中利用相对运动原理，将各坐标系的实际运动情况转换成SLAM算法适用的场景：B坐标系与C0坐标系静止，C坐标系绕B坐标系转动；估计出每帧C坐标系相对于C0坐标系的旋转矩阵R_k和平移矩阵t_k；

具体过程为：

步骤三一、RGBD相机采集彩色图像和深度图像，将RGBD相机采集的彩色图像输入训练好的YOLO目标检测模型进行实时目标识别与检测，若检测到某帧彩色图像的confidence值大于0.95，则停止YOLO模型检测，并记录YOLO输出的此帧彩色图像的x′，y′，w′，h值，将x′，y′，w′，h值构成的区域记作ROI区域；

所述confidence为置信度，x′，y′为目标所在图像像素区域的中心坐标值，w′为目标所在图像像素区域的宽，h为目标所在图像像素区域的高；

将目标检测的流程统一为单个神经网络，该神经网络采用整个图像信息来预测目标的bounding boxes的同时识别目标的类别，实现端到端实时目标检测任务。YOLO首先将图像分为S×S的格子(grid cell)。如果一个目标的中心落入格子，该格子就负责检测该目标。每一个格子(grid cell)预测bounding boxes(B)和该boxes的置信值(confidencescore)。置信值代表box包含一个目标的置信度。然后，定义置信值。如果没有目标，置信值为零。另外，希望预测的置信值和ground truth的intersection overunion(IOU)相同。每一个bounding box包含5个值：x，y，w，h和confidence。(x，y)代表与格子相关的box的中心。(w，h)为与全图信息相关的box的宽和高。confidence代表预测boxes的IOU和gound truth。

yolo检测一个目标所在图像的像素区域，这个像素区域的中心是x,y；像素区域的宽和高是wh。但是xywh都是一个比值，是这个框的实际的中心坐标、框的宽高与整个图像的一个比值。

相机与非合作目标位置相对静止，故确定一帧非合作目标所在像素区域，其他帧图片非合作目标所在的区域也已知，不需要再次检测。

步骤三二、若RGBD相机采集到的彩色图像是confidence值大于0.95后的第i帧图像，i＝1；

提取彩色图像的confidence值大于0.95后的第i帧图像ROI区域中的ORB特征点(Oriented FAST关键点和Rotated BRIEF描述子)，以C0坐标系为世界坐标系，根据深度图像的信息将ORB特征点转换为世界坐标系下的三维地图点，放入局部地图中，并将彩色图像的confidence值大于0.95后的第一帧设为关键帧；

所述第i帧图像为彩色图像的confidence值大于0.95后的第i帧图像；

关键帧的选取原则：距上一关键帧插入已经超过20帧，或者当前帧的ORB特征点与地图点成功匹配上了至少50个点；

步骤三三、若RGBD相机采集到的彩色图像是confidence值大于0.95后的第i帧图像，i≠1；

提取彩色图像的confidence值大于0.95后的第i帧(该帧)图像ROI区域中的ORB特征点；

首先，局部地图中的地图点与彩色图像的confidence值大于0.95后的第i帧(该帧)图像的ORB特征点进行匹配，并更新局部地图：

1)给定一个阈值，当匹配上的点大于阈值，删除局部地图中不在当前帧相机视野内的地图点；

2)当匹配上的点小于等于阈值时，将当前帧的所有的ORB特征点转换成地图点，放入到局部地图中；

3)对于关键帧，要将关键帧中所有的ORB特征点转换为地图点存入局部地图；

然后，根据匹配好的地图点与第i帧图像的ORB特征点，采用3D-2D:PnP方法，计算出第i帧图像的R_i和t_i，并将第1至第i帧的R_k和t_k传入步骤三四的图优化中进行后端优化；

3D-2D(PnP)

相机将三维世界中的坐标点映射到二维图像平面的过程可以用一个几何模型进行描述，这个模型有很多种，其中最简单的是针孔模型，它描述了一束光线通过针孔后在针孔背面投影成像的过程。

以u,v表示P点的像素坐标，以X,Y,Z表示该像素点在相机坐标系下的坐标。则有：

其中,f_x,f_y,c_x,c_y是相机的内参系数，相机内参系数构成的矩阵在下文我们用K表示

令

为像素点的归一化坐标。

相机的外参是相机坐标系与世界坐标系的转换矩阵R、t，其中R是旋转矩阵(3*3)，t为平移矩阵(3*1)。设P点在世界坐标系的坐标为X_W,Y_W,Z_W,则：

用归一化坐标来表示：

3D-2D问题描述为给定两组特征点集，互为匹配，其中一组为3D真实坐标，另一组为二维像素坐标，求两者之间的运动估计。简单的说，3D-2D的运动估计是从3D空间点X_K-1到对应二维像素点p_K之间的运动估计，得到两帧间相机的旋转矩阵R_K和平移矩阵t_K。在该算法中，我们主要是匹配局部地图上的3D点与二维图像上的ORB特征点。一般描述成

其中

是三维空间点[X Y Z]^T根据相机运动矩阵重投影到下一帧图像中的像素坐标。

设[x y I]^T是像素坐标系上点的归一化坐标，[X Y Z]^T是世界坐标系上的三维点，则上式最小时等式为0，我们定义增广矩阵T_K＝[R_K|t_K]为3×4的矩阵。

令上式为0，用归一化坐标的形式可以写成：

将其展开，令T¹，T²，T³分别为T_K的第一行，第二行，第三行，再通过线性变换的算法求得转换矩阵T_K＝[R_K t_K]。

当恰好有6个点对时，则可以得到一个齐次线性方程组AT＝0，对A做SVD即可求解出T_k，进而求得旋转矩阵R_K和平移矩阵t_K。

将局部地图上的三维点与每一帧图像上的ORB特征点进行3D-2D计算，可得到每一帧相机坐标系的旋转矩阵和平移矩阵。

步骤三四、建立因子图，用g2o对因子图进行后端优化，即对第1至第i帧图像的C坐标系相对于C₀坐标系的旋转矩阵R_k、平移矩阵t_k和局部地图上的地图点进行优化，得到优化后的旋转矩阵R_k、平移矩阵t_k和局部地图上的地图点，k＝1,2,…,i；

步骤三五、若检测到的彩色图像的confidence值大于0.95后的第i帧为被检测帧，则基于词袋模型进行回环检测，判断第i帧是否能检测出回环，若是，再次优化第1至第i帧图像的C坐标系相对于C₀坐标系的旋转矩阵R_k、平移矩阵t_k和局部地图上的地图点(步骤三五)；若否，则执行步骤四；

若第i帧不是被检测帧，则执行步骤四；

步骤四、对YOLO目标检测模型输出的彩色图像的confidence值大于0.95后RGBD相机采集的每帧图像，均执行一次步骤三二至步骤三五，直至检测出两次回环或达到指定时间或帧数，RGBD相机停止采集图像；

步骤五、C坐标系相对C₀坐标系的运动轨迹为圆弧，设C₀坐标系的原点为p(0,0,0)，根据步骤四中估计出的每帧C坐标系相对于C₀坐标系的平移矩阵t_k，设每帧C坐标系的原点在C₀坐标系下为t_cc0，t_cc0＝p+t_k；

彩色图像的confidence值大于0.95后的n帧图像C坐标系在C₀坐标系下的原点三维坐标(记作

t_cc01 ¹表示第1帧C坐标系原点的x值，依次类推，右上角的123表示xyz，右下角的123...n表示第几帧)可以拟合成空间平面和空间圆弧；

所述n帧为RGBD相机停止采集图像前彩色图像的confidence值大于0.95后的所有帧；

拟合成的空间平面的法线是非合作目标转轴的斜率，空间圆弧的圆心是非合作目标转轴经过的点。

具体实施方式二：本实施方式与具体实施方式一不同的是：所述步骤一中利用ImageNet数据库对YOLO目标检测模型做预训练；将前期获得的非合作目标图片输入预训练好的YOLO目标检测模型进行训练，得到训练好的YOLO目标检测模型；具体过程为：

YOLO目标检测模型是一个卷积神经网络，它包括24个卷积层、4个池化层和2个全连接层，分别是：

1个卷积核尺寸为7*7，个数为64的卷积层；

1个2*2的最大池化层；

1个卷积核尺寸为3*3，个数为192的卷积层；

1个2*2的最大池化层；

1个卷积核尺寸为1*1，个数为128的卷积层；

1个卷积核尺寸为3*3，个数为256的卷积层；

1个卷积核尺寸为1*1，个数为256的卷积层；

1个卷积核尺寸为3*3，个数为512的卷积层；

1个2*2的最大池化层；

4个卷积核尺寸为1*1，个数为256的卷积层；

4个卷积核尺寸为3*3，个数为512的卷积层；

1个卷积核尺寸为1*1，个数为512的卷积层；

1个卷积核尺寸为3*3，个数为1024的卷积层；

1个2*2的最大池化层；

2个卷积核尺寸为1*1，个数为512的卷积层；

2个卷积核尺寸为3*3，个数为1024的卷积层；

2个卷积核尺寸为3*3，个数为1024的卷积层；

2个卷积核尺寸为3*3，个数为1024的卷积层；

2个全连接层；

利用ImageNet数据库的数据对YOLO目标检测模型做预训练，得到YOLO目标检测模型各层的初始权值，然后将前期获得的非合作目标图片(数量大于300，且已做好标签)输入到YOLO目标检测模型，不断进行迭代训练调整YOLO目标检测模型权值，直到YOLO目标检测模型的损失函数值小于给定阈值0.2，得到此时YOLO目标检测模型每一层的权值，根据此时YOLO目标检测模型每一层的权值得到训练好的YOLO目标检测模型。

其它步骤及参数与具体实施方式一相同。

具体实施方式三：本实施方式与具体实施方式一或二不同的是：所述获得的非合作目标图片的尺寸为448×448，图片数量大于800张，且已做好标签。

其它步骤及参数与具体实施方式一或二相同。

具体实施方式四：本实施方式与具体实施方式一至三之一不同的是：所述步骤三四中建立因子图，用g2o对因子图进行后端优化，即对第1至第i帧图像的C坐标系相对于C₀坐标系的旋转矩阵R_k、平移矩阵t_k和局部地图上的地图点进行优化，得到优化后的旋转矩阵R_k、平移矩阵t_k和局部地图上的地图点，k＝1,2,…,i；具体过程为：

第k帧图像的变换矩阵为T_k；即当模型输入第i帧图像时，对前面的i帧图像均引入变换矩阵，

其中：R_k是第k帧图像对应的旋转矩阵、t_k是第k帧图像对应的平移矩阵；0是一个3*1维的0矩阵，0^T是0矩阵的转置矩阵；

设第i帧对应的局部地图上的地图点共有w个；

将第1帧至第i帧图像的C坐标系相对于C₀坐标系的变换矩阵T₁,T₂,…T_i和局部地图中每一个地图点y_j分别作为因子图中被优化的变量节点，分别记为变量节点P1，P2，…，Pk，…，Pi和Q1，Q2，…，Qj，…，Qw，j＝1,2,…,w；

将

作为因子图中变量节点Pk和变量节点Qj之间的因子节点；

其中：(u′、v′)表示第k帧图像和局部地图中的地图点y_j相匹配的ORB特征点的坐标；(u、v)表示局部地图上的地图点y_j经过变换矩阵T_k投影到像素坐标系上的坐标；||·||²是二范数；

局部地图上的地图点y_j经过变换矩阵T_k投影到像素坐标系：

(X_j,Y_j,Z_j)表示局部地图上的地图点y_j在世界坐标系下的三维坐标；f_x、f_y、c_x、c_y为相机的内参系数；

根据因子节点、变量节点建立一个因子图，用g2o对因子图进行后端优化；

得到优化后第1至第i帧图像的C坐标系相对于C₀坐标系的旋转矩阵R_k、平移矩阵t_k和局部地图上的地图点，记作中间变量X^*，

其中

T_1:i是T₁,T₂,…,T_i组成的矩阵，y_1:w是y₁，y₂，…，y_w组成的矩阵。

g2o图优化过程介绍如下：

如果只使用PnP来估计位姿，会使估计的结果有累积误差，进而产生轨迹漂移。因此需要在一段时间后对所有估计出的相机位姿(R_K，t_K)和局部地图上的地图点进行全局优化。

假设，相机在位姿x_k时，测地图点y_j在像素坐标系下的坐标为z_kj,此时有测量误差v_kj，并假设测量误差服从高斯分布，对于一次观测：

z_kj＝h(y_j,x_k)+v_kj

假设噪声项v_kj～N(0,Q_kj)所以，观测数据的条件概率为：P(z_kj|x_k，y_j)＝N(h(y_j，x_k)，Q_kj)；

它依然是一个高斯分布，为了计算使它最大化的x_k和y_j，我们用最小化负对数的方式来求高斯分布的最大似然。

考虑任意高维高斯分布x～N(0，Q_kj)，将其概率密度函数取负对数，则有：

对原分布求最大化，相当于对负对数求最小化，最小化上式的x时，第一项与x无关，可以去掉，只需优化右侧的二次型式。带入观测方程，相当于求：

x^*＝argmin((z_kj-h(y_j,x_k))^TQ_kj ^-1(z_kj-h(y_j,x_k)))

令z_kj-h(y_j，x_k)＝e_jk，考虑到所有帧中的路标点和位姿，则误差函数可以转换成下式：

即找到适当的x_k和y_j使上式最小，转换成了非线性的最小二乘问题。

因子图是一种无向图，由两种节点组成：表示优化变量的变量节点和表示因子的因子节点。被优化的变量节点用圆圈表示，因子用矩形表示。一个因子连接着几个节点，表示这几个节点之间的联合概率分布。在位姿估计中，被优化量即为x与y，因子为误差ejk。因子图优化的实质是，将优化问题转换成一个加权非线性最小二乘问题，通过调整各节点的值，使系统的误差函数值最小。

设X_k为从时刻0到时刻k所有的位姿构成的矩阵；Y_j为从地图点0到地图点j所有的地图点构成的矩阵；X为X_k和Y_j构成的矩阵。

假设

和

是一个初始估计，存在一个小增量ΔX_k和ΔY_j。则e_jk可以写成如下形式：

其中，J_jk是e_jk在

处的雅克比矩阵。当有增量时，误差函数F(x)的每个小分量可表示如下：

将式带入得：

用

代替

则有：

将

用C_jk表示，将

用b_jk表示，将J_jk ^TQ_kj ^-1J_jk用H_jk表示，则上式可写成如下的简化形式：

那么，

所以，我们的目的是找到ΔX使误差的增量最小，即：

得到

HΔX^*＝-b

要求的解X^*＝X+ΔX^*。

当要求解的位姿和地图点数量非常大时，H矩阵求逆会变得很复杂，要充分利用H矩阵的稀疏性和边缘性来求解。

这样，我们得到被优化的所有的位姿矩阵和所有的局部地图上的地图点。

其它步骤及参数与具体实施方式一至三之一相同。

具体实施方式五：本实施方式与具体实施方式一至四之一不同的是：所述步骤三五中若检测到的彩色图像的confidence值大于0.95后的第i帧为被检测帧，则基于词袋模型进行回环检测，判断第i帧是否能检测出回环，若是，再次优化第1至第i帧图像的C坐标系相对于C₀坐标系的旋转矩阵R_k、平移矩阵t_k和局部地图上的地图点(步骤三五)；若否，则执行步骤四；若第i帧不是被检测帧，则执行步骤四；具体过程为：

回环检测的具体流程如下：

被检测帧选取原则：根据步骤三四输出的被优化后的第i帧图像的C坐标系相对C0坐标系的旋转矩阵R_k、采集第1帧图像到第i帧图像所用的时间，预估转速w_i，根据转速w_i估计发生回环时间t，若第1帧图像到第i帧图像所用的时间在0.7t至1.3t的时间范围内，则该帧图像设置为被检测帧；

若检测到的彩色图像的confidence值大于0.95后的第i帧为被检测帧，则基于词袋模型进行回环检测，过程为；

步骤二建立完k叉数类型的字典后，利用正向索引和反向索引对被检测帧图像的任意特征f_i找到字典中对应的单词w_j，根据每个单词出现的频率利用TF-IDF进行评分，得到用向量表示的被检测帧图像在字典中单词列表的分布；

计算评分后被检测帧和第一帧之间的相似度：

假设第一帧图像通过TF-IDF评分后用向量v_A来表示，被检测帧通过TF-IDF评分后用向量v_B来表示，则向量v_A和向量v_B对应的两帧间的相似度为：

其中：v_Ai′是v_A中的第i′个元素，v_Bi′是v_B中的第i′个元素，i′＝1,2,…,N′；|v_Ai′|和|v_Bi′|分别是v_Ai′和v_Bi′的绝对值，|v_Ai′-v_Bi′|是v_Ai′与v_Bi′的差的绝对值；N′为被检测帧的总特征个数；

给定阈值，若相似度大于阈值，则能检测出回环，再次优化第1至第i帧图像的C坐标系相对于C₀坐标系的旋转矩阵R_k、平移矩阵t_k和局部地图上的地图点；若否，则执行步骤四；若第i帧不是被检测帧，则执行步骤四；

所述0≤阈值≤1。

其它步骤及参数与具体实施方式一至四之一相同。

具体实施方式六：本实施方式与具体实施方式一至五之一不同的是：所述再次优化第1至第i帧图像的C坐标系相对于C₀坐标系的旋转矩阵R_k、平移矩阵t_k和局部地图上的地图点；具体过程为：

以||T_A-T_K||²作为新的因子节点加入到变量节点P1和变量节点PK之间，进一步优化第1帧到第i帧图像的C坐标系相对于C₀坐标系的旋转矩阵R_k、平移矩阵t_k和局部地图中的地图点(重新执行步骤三五)；

T_A为第一帧图像的变换矩阵，T_K为检测出发生回环的被检测帧K的变换矩阵。

其它步骤及参数与具体实施方式一至五之一相同。

具体实施方式七：本实施方式与具体实施方式一至六之一不同的是：所述根据转速ω_i估计发生回环时间t，公式为：

其它步骤及参数与具体实施方式一至六之一相同。

具体实施方式八：本实施方式与具体实施方式一至七之一不同的是：所述步骤五中C坐标系相对C₀坐标系的运动轨迹为圆弧，设C₀坐标系的原点为p(0,0,0)，根据步骤三中估计出的每帧C坐标系相对于C₀坐标系的平移矩阵t_k，设每帧C坐标系的原点在C₀坐标系下为t_cc0，t_cc0＝p+t_k；

n帧图像C坐标系在C₀坐标系下的原点三维坐标(记作

t_cc01 ¹表示第1帧C坐标系原点的x值，依次类推，右上角的123表示xyz，右下角的123...n表示第几帧)可以拟合成空间平面和空间圆弧；

拟合成的空间平面的法线是非合作目标转轴的斜率，空间圆弧的圆心是非合作目标转轴经过的点；

具体过程为：

空间平面拟合：

空间平面方程满足：

Ax+By+Cz+D＝0

A、B、C、D为空间平面方程系数，x为C坐标系的原点x方向的值，y为C坐标系的原点y方向的值，z为C坐标系的原点z方向的值；

已知n帧C坐标系的原点均在空间平面上，则有：

其中，x₁为第1帧C坐标系原点的x方向坐标，x₂为第1帧C坐标系原点的x方向坐标，x_n为第1帧C坐标系原点的x方向坐标，y₁为第1帧C坐标系原点的y方向坐标，y₂为第1帧C坐标系原点的y方向坐标，y_n为第1帧C坐标系原点的y方向坐标，z₁为第1帧C坐标系原点的z方向坐标，z₂为第1帧C坐标系原点的z方向坐标，z_n为第1帧C坐标系原点的z方向坐标；

另上式记作AX＝0，已知矩阵A(即为上步的P矩阵最后一列加1)，采用奇异值分解(SVD)求解X，因为X＝0时上式恒成立，所以加上限制||X||＝1；

采用奇异值分解(SVD)求解X，根据X拟合成空间平面，空间平面的法线即非合作目标转轴的斜率:[AB C]^T；

空间圆弧拟合：

随着时间的推移，C坐标系的原点形成的轨迹在C0坐标系下为空间球面的一段圆弧。空间圆弧的方程可用一个空间球面和一个空间平面来表示，如下式：

设空间球面的圆心坐标为(x₀,y₀,z₀)，球面的半径为r₀，空间平面方程的系数为A、B、C、D；

C坐标系原点的x方向坐标构成矩阵X(矩阵P的第一列)，y方向坐标构成矩阵Y(矩阵P的第二列)，z方向坐标构成矩阵Z(矩阵P的第三列)，由点拟合出的空间平面上一节已经求出，对C坐标系原点进行空间球面拟合，即满足下式：

拟合的方法如下：

先求取n个C坐标系原点的重心，作为球面的初始球心坐标，以n个C坐标系的原点到重心的平均距离为初始半径，利用梯度下降法，不断调整x₀,y₀,z₀,r₀的值，使误差函数(公式2)最小，求出空间球面的方程(满足条件的空间球面有无数种情况，这里随便选一种)；

已知球面方程(记球心为O坐标表示为(x₀,y₀,z₀))和平面方程，平面方程记为平面α，求取空间圆弧的圆心O′和空间圆弧的坐标(x₀′,y₀′,z₀′)，如图4，方法如下：

空间圆弧的圆心O′可以看成球心O在平面α的投影点，所以向量

与平面α的法向量平行，即：

由投影原理知，

K为中间变量；

将式(3)带入式(4)得到圆弧的圆心坐标：

虽然由圆弧状分布的空间点拟合出的空间球面有无数个，但是该段圆弧的圆心坐标是唯一的。

求出的圆弧圆心坐标为非合作目标转轴所过的点；

综上，非合作目标转轴所过的点(x₀′,y₀′,z₀′)已知，非合作目标转轴的斜率[A BC]^T已知，非合作目标转轴的直线方程可用点斜式来表示：

其它步骤及参数与具体实施方式一至七之一相同。

具体实施方式九：本实施方式与具体实施方式一至八之一不同的是：所述采用奇异值分解(SVD)求解X，具体过程为：

将矩阵A进行奇异值分解(SVD)分解，写成A＝U∑V^T的形式，

其中，U为AA^T的特征向量组成的正交矩阵，∑为AA^T的特征值的平方根组成的对角阵，V为A^TA的特征向量组成的正交矩阵；T为转置；

所以采用奇异值分解(SVD)求解X转换为使||U∑V^TX||最小，而U∑V^TX||＝||∑V^TX||，||V^TX||＝||X||＝1；

设Y＝V^TX，问题转换成||∑ Y||最小，因为∑对角线元素按从大到小的顺序排列。显然，为了使||∑ Y||最小，Y＝[0 0 0 … 1]^T，再由X＝VY求解出X，X的值就是V的最后一列。

其它步骤及参数与具体实施方式一至八之一相同。

采用以下实施例验证本发明的有益效果：

实施例一：

本实施例具体是按照以下步骤制备的：

实验系统主要由GPU工作站、Kinect 2深度相机、非合作目标、电动转台、电源构成，各构件的连接如图5所示。非合作目标放在匀速旋转的电动转台上，以0.2166rad/s的速度匀速转动；Kinect2相机实时采集匀速旋转的非合作目标的深度图像和彩色图像，传入GPU工作站，经过YOLO卷积神经网络对非合作目标正确识别后，确定目标所在的像素区域，开始对非合作目标进行转轴估算。

转轴斜率估计结果

在实验期间，规定采集683帧图像，非合作目标的体坐标系与相机坐标系平行，非合作目标绕竖直轴顺时针转动，相机坐标系定义y轴向下。所以，转轴直线的斜率在相机坐标系下的真实值应为[0 -1 0]^T。将每帧时算法测出的斜率和真实的斜率绘图，如图6：

(2)圆弧圆心估计结果：

以规定帧数为50帧、100帧、150帧、200帧为例

算法测出的C坐标系原点在MATLAB上的绘图、C坐标系原点的原点用空间球面拟合的效果图如图7a、7b、8a、8b、9a、9b、10a、10b，对相机坐标系原点同时拟合出的空间球面与空间平面的效果图如图11、12、13、14；

空间三点便可确定一个圆，绘出从第3帧到规定帧数683帧时拟合出的空间圆的圆心x₀、y₀、z₀,以及他们的真实值，如图15。

测量值与真实值之间的相对误差(真实值与测量值的差)，如图16(误差单位为cm)：

本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (9)

1.基于视觉SLAM估计空间旋转非合作目标转轴的方法，其特征在于：所述方法具体过程为：

步骤一、利用ImageNet数据库对YOLO目标检测模型做预训练；将前期获得的非合作目标图片输入预训练好的YOLO目标检测模型进行训练，得到训练好的YOLO目标检测模型；

步骤二、对前期获得的非合作目标图片信息提取ORB特征点，利用K均值算法进行聚类建立k叉数字典；

步骤三、设三个坐标系，分别是C坐标系、C₀坐标系、B坐标系；

C坐标系为相机坐标系；B坐标系为非合作目标本体坐标系，且过非合作目标质心；C₀坐标系与B坐标系运动同步，且相机采集到第一帧置信值大于0.95的图像时，C₀坐标系与C坐标系重合；

各坐标系的实际运动情况为：以地面为参考系C坐标系静止；B坐标系以恒定角速度自旋；C₀坐标系的运动与B坐标系转动同步，随时间移动，C₀坐标系原点的轨迹呈圆弧；

利用相对运动原理，将各坐标系的实际运动情况转换成SLAM算法适用的场景：B坐标系与C₀坐标系静止，C坐标系绕B坐标系转动；估计出每帧C坐标系相对于C₀坐标系的旋转矩阵R_k和平移矩阵t_k；具体过程为：

步骤三一、RGBD相机采集彩色图像和深度图像，将RGBD相机采集的彩色图像输入训练好的YOLO目标检测模型进行实时目标识别与检测，若检测到某帧彩色图像的confidence值大于0.95，则停止YOLO模型检测，并记录YOLO输出的此帧彩色图像的x′，y′，w′，h值，将x′，y′，w′，h值构成的区域记作ROI区域；

所述confidence为置信度，x′，y′为目标所在图像像素区域的中心坐标值，w′为目标所在图像像素区域的宽，h为目标所在图像像素区域的高；

步骤三二、若RGBD相机采集到的彩色图像是confidence值大于0.95后的第i帧图像，i＝1；

提取彩色图像的confidence值大于0.95后的第i帧图像ROI区域中的ORB特征点，以C₀坐标系为世界坐标系，根据深度图像的信息将ORB特征点转换为世界坐标系下的三维地图点，放入局部地图中，并将彩色图像的confidence值大于0.95后的第一帧设为关键帧；

所述第i帧图像为彩色图像的confidence值大于0.95后的第i帧图像；

关键帧的选取原则：距上一关键帧插入已经超过20帧，或者当前帧的ORB特征点与地图点成功匹配上了至少50个点；

步骤三三、若RGBD相机采集到的彩色图像是confidence值大于0.95后的第i帧图像，i≠1；

提取彩色图像的confidence值大于0.95后的第i帧图像ROI区域中的ORB特征点；

首先，局部地图中的地图点与彩色图像的confidence值大于0.95后的第i帧图像的ORB特征点进行匹配，并更新局部地图：

1)给定一个阈值，当匹配上的点大于阈值，删除局部地图中不在当前帧相机视野内的地图点；

2)当匹配上的点小于等于阈值时，将当前帧的所有的ORB特征点转换成地图点，放入到局部地图中；

3)对于关键帧，要将关键帧中所有的ORB特征点转换为地图点存入局部地图；

然后，根据匹配好的地图点与第i帧图像的ORB特征点，采用3D-2D:PnP方法，计算出第i帧图像的R_i和t_i，并将第1至第i帧的R_k和t_k传入步骤三四的图优化中进行后端优化；

步骤三四、建立因子图，用g2o对因子图进行后端优化，即对第1至第i帧图像的C坐标系相对于C₀坐标系的旋转矩阵R_k、平移矩阵t_k和局部地图上的地图点进行优化，得到优化后的旋转矩阵R_k、平移矩阵t_k和局部地图上的地图点，k＝1,2,…,i；

步骤三五、若检测到的彩色图像的confidence值大于0.95后的第i帧为被检测帧，则基于词袋模型进行回环检测，判断第i帧是否能检测出回环，若是，再次优化第1至第i帧图像的C坐标系相对于C₀坐标系的旋转矩阵R_k、平移矩阵t_k和局部地图上的地图点；若否，则执行步骤四；

若第i帧不是被检测帧，则执行步骤四；

步骤四、对YOLO目标检测模型输出的彩色图像的confidence值大于0.95后RGBD相机采集的每帧图像，均执行一次步骤三二至步骤三五，直至检测出两次回环或达到指定时间或帧数，RGBD相机停止采集图像；

步骤五、C坐标系相对C₀坐标系的运动轨迹为圆弧，设C₀坐标系的原点为p(0,0,0)，根据步骤四中估计出的每帧C坐标系相对于C₀坐标系的平移矩阵t_k，设每帧C坐标系的原点在C₀坐标系下为t_cc0，t_cc0＝p+t_k；

彩色图像的confidence值大于0.95后的n帧图像C坐标系在C₀坐标系下的原点三维坐标拟合成空间平面和空间圆弧；

所述n帧为RGBD相机停止采集图像前彩色图像的confidence值大于0.95后的所有帧；

拟合成的空间平面的法线是非合作目标转轴的斜率，空间圆弧的圆心是非合作目标转轴经过的点。

2.根据权利要求1所述基于视觉SLAM估计空间旋转非合作目标转轴的方法，其特征在于：所述步骤一中利用ImageNet数据库对YOLO目标检测模型做预训练；将前期获得的非合作目标图片输入预训练好的YOLO目标检测模型进行训练，得到训练好的YOLO目标检测模型；具体过程为：

YOLO目标检测模型是一个卷积神经网络，它包括24个卷积层、4个池化层和2个全连接层，分别是：

1个卷积核尺寸为7*7，个数为64的卷积层；

1个2*2的池化层；

1个卷积核尺寸为3*3，个数为192的卷积层；

1个2*2的池化层；

1个卷积核尺寸为1*1，个数为128的卷积层；

1个卷积核尺寸为3*3，个数为256的卷积层；

1个卷积核尺寸为1*1，个数为256的卷积层；

1个卷积核尺寸为3*3，个数为512的卷积层；

1个2*2的池化层；

4个卷积核尺寸为1*1，个数为256的卷积层；

4个卷积核尺寸为3*3，个数为512的卷积层；

1个卷积核尺寸为1*1，个数为512的卷积层；

1个卷积核尺寸为3*3，个数为1024的卷积层；

1个2*2的池化层；

2个卷积核尺寸为1*1，个数为512的卷积层；

2个卷积核尺寸为3*3，个数为1024的卷积层；

2个卷积核尺寸为3*3，个数为1024的卷积层；

2个卷积核尺寸为3*3，个数为1024的卷积层；

2个全连接层；

利用ImageNet数据库的数据对YOLO目标检测模型做预训练，得到YOLO目标检测模型各层的初始权值，然后将前期获得的非合作目标图片输入到YOLO目标检测模型，不断进行迭代训练调整YOLO目标检测模型权值，直到YOLO目标检测模型的损失函数值小于给定阈值0.2，得到此时YOLO目标检测模型每一层的权值，根据此时YOLO目标检测模型每一层的权值得到训练好的YOLO目标检测模型。

3.根据权利要求2所述基于视觉SLAM估计空间旋转非合作目标转轴的方法，其特征在于：所述前期获得的非合作目标图片的尺寸为448×448，图片数量大于800张，且已做好标签。

4.根据权利要求3所述基于视觉SLAM估计空间旋转非合作目标转轴的方法，其特征在于：所述步骤三四中建立因子图，用g2o对因子图进行后端优化，即对第1至第i帧图像的C坐标系相对于C₀坐标系的旋转矩阵R_k、平移矩阵t_k和局部地图上的地图点进行优化，得到优化后的旋转矩阵R_k、平移矩阵t_k和局部地图上的地图点，k＝1,2,…,i；具体过程为：

第k帧图像的变换矩阵为T_k；

其中：R_k是第k帧图像对应的旋转矩阵、t_k是第k帧图像对应的平移矩阵；0是一个3*1维的0矩阵，0^T是0矩阵的转置矩阵；

设第i帧对应的局部地图上的地图点共有w个；

将第1帧至第i帧图像的C坐标系相对于C₀坐标系的变换矩阵T₁,T₂,…T_i和局部地图中每一个地图点y′_j分别作为因子图中被优化的变量节点，分别记为变量节点P1，P2，…，Pk，…，Pi和Q1，Q2，…，Qj，…，Qw，j＝1,2,…,w；

将

作为因子图中变量节点Pk和变量节点Qj之间的因子节点；

其中：(u′、v′)表示第k帧图像和局部地图中的地图点y′_j相匹配的ORB特征点的坐标；(u、v)表示局部地图上的地图点y′_j经过变换矩阵T_k投影到像素坐标系上的坐标；||·||²是二范数；

局部地图上的地图点y′_j经过变换矩阵T_k投影到像素坐标系：

(X_j,Y_j,Z_j)表示局部地图上的地图点y′_j在世界坐标系下的三维坐标；f_x、f_y、c_x、c_y为相机的内参系数；

根据因子节点、变量节点建立一个因子图，用g2o对因子图进行后端优化；

得到优化后第1至第i帧图像的C坐标系相对于C₀坐标系的旋转矩阵R_k、平移矩阵t_k和局部地图上的地图点，记作中间变量X^*，

其中

T_1:i是T₁,T₂,…,T_i组成的矩阵，y_1:w是y1,y2,…,yw组成的矩阵。

5.根据权利要求4所述基于视觉SLAM估计空间旋转非合作目标转轴的方法，其特征在于：所述步骤三五中若检测到的彩色图像的confidence值大于0.95后的第i帧为被检测帧，则基于词袋模型进行回环检测，判断第i帧是否能检测出回环，若是，再次优化第1至第i帧图像的C坐标系相对于C₀坐标系的旋转矩阵R_k、平移矩阵t_k和局部地图上的地图点；若否，则执行步骤四；若第i帧不是被检测帧，则执行步骤四；具体过程为：

被检测帧选取原则：根据步骤三四输出的被优化后的第i帧图像的C坐标系相对C₀坐标系的旋转矩阵R_k、采集第1帧图像到第i帧图像所用的时间，预估转速w_i，根据转速w_i估计发生回环时间t，若第1帧图像到第i帧图像所用的时间在0.7t至1.3t的时间范围内，则该帧图像设置为被检测帧；

若检测到的彩色图像的confidence值大于0.95后的第i帧为被检测帧，则基于词袋模型进行回环检测，过程为；

步骤二建立完k叉数类型的字典后，利用正向索引和反向索引对被检测帧图像的任意特征f_i找到字典中对应的单词w_j，根据每个单词出现的频率利用TF-IDF进行评分，得到用向量表示的被检测帧图像在字典中单词列表的分布；

计算评分后被检测帧和第一帧之间的相似度：

假设第一帧图像通过TF-IDF评分后用向量v_A来表示，被检测帧通过TF-IDF评分后用向量v_B来表示，则向量v_A和向量v_B对应的两帧间的相似度为：

其中：v_Ai′是v_A中的第i′个元素，v_Bi′是v_B中的第i′个元素，i′＝1,2,…,N′；|v_Ai′|和|v_Bi′|分别是v_Ai′和v_Bi′的绝对值，|v_Ai′-v_Bi′|是v_Ai′与v_Bi′的差的绝对值；N′为被检测帧的总特征个数；

给定阈值，若相似度大于阈值，则能检测出回环，再次优化第1至第i帧图像的C坐标系相对于C₀坐标系的旋转矩阵R_k、平移矩阵t_k和局部地图上的地图点；若否，则执行步骤四；若第i帧不是被检测帧，则执行步骤四；

所述阈值取值范围为：0≤阈值≤1。

6.根据权利要求5所述基于视觉SLAM估计空间旋转非合作目标转轴的方法，其特征在于：所述再次优化第1至第i帧图像的C坐标系相对于C₀坐标系的旋转矩阵R_k、平移矩阵t_k和局部地图上的地图点；具体过程为：

以||T_A-T_K||²作为新的因子节点加入到变量节点P1和变量节点PK之间，进一步优化第1帧到第i帧图像的C坐标系相对于C₀坐标系的旋转矩阵R_k、平移矩阵t_k和局部地图中的地图点；

T_A为第一帧图像的变换矩阵，T_K为检测出发生回环的被检测帧K的变换矩阵。

7.根据权利要求6所述基于视觉SLAM估计空间旋转非合作目标转轴的方法，其特征在于：所述根据转速w_i估计发生回环时间t，公式为：

8.根据权利要求7所述基于视觉SLAM估计空间旋转非合作目标转轴的方法，其特征在于：所述步骤五中C坐标系相对C₀坐标系的运动轨迹为圆弧，设C₀坐标系的原点为p(0,0,0)，根据步骤三中估计出的每帧C坐标系相对于C₀坐标系的平移矩阵t_k，设每帧C坐标系的原点在C₀坐标系下为t_cc0，t_cc0＝p+t_k；

n帧图像C坐标系在C₀坐标系下的原点三维坐标拟合成空间平面和空间圆弧；

拟合成的空间平面的法线是非合作目标转轴的斜率，空间圆弧的圆心是非合作目标转轴经过的点；

具体过程为：

空间平面拟合：

空间平面方程满足：

A′x+B′y+C′z+D＝0

A′、B′、C′、D为空间平面方程系数，x为C坐标系的原点x方向的值，y为C坐标系的原点y方向的值，z为C坐标系的原点z方向的值；

已知n帧C坐标系的原点均在空间平面上，则有：

其中，x₁为第1帧C坐标系原点的x方向坐标，x₂为第2帧C坐标系原点的x方向坐标，x_n为第n帧C坐标系原点的x方向坐标，y₁为第1帧C坐标系原点的y方向坐标，y₂为第2帧C坐标系原点的y方向坐标，y_n为第n帧C坐标系原点的y方向坐标，z₁为第1帧C坐标系原点的z方向坐标，z₂为第2帧C坐标系原点的z方向坐标，z_n为第n帧C坐标系原点的z方向坐标；

另上式记作AX′＝0，已知矩阵A，采用奇异值分解求解X′，因为X′＝0时上式恒成立，所以限制||X′||＝1；

采用奇异值分解求解X′，根据X′拟合成空间平面，空间平面的法线即非合作目标转轴的斜率:[A′ B′ C′]^T；

空间圆弧拟合：

空间圆弧的方程用一个空间球面和一个空间平面来表示，如下式：

设空间球面的圆心坐标为(x₀,y₀,z₀)，球面的半径为r₀，空间平面方程的系数为A′、B′、C′、D；

C坐标系原点的x方向坐标构成X，y方向坐标构成Y，z方向坐标构成Z，对C坐标系原点进行空间球面拟合，即满足下式：

拟合的方法如下：

先求取n个C坐标系原点的重心，作为球面的初始球心坐标，以n个C坐标系的原点到重心的平均距离为初始半径，利用梯度下降法，不断调整x₀,y₀,z₀,r₀的值，使误差函数公式2最小，求出空间球面的方程；

已知球面方程和平面方程α，求取空间圆弧的圆心O′和空间圆弧的坐标(x₀′,y₀′,z₀′)，方法如下：

空间圆弧的圆心O′看成球心O在平面α的投影点，所以向量

与平面α的法向量平行，即：

由投影原理知，

K′为中间变量；

将式(3)带入式(4)得到圆弧的圆心坐标：

求出的圆弧圆心坐标为非合作目标转轴所过的点；

综上，非合作目标转轴所过的点(x₀′,y₀′,z₀′)已知，非合作目标转轴的斜率[A′ B′C′]^T已知，非合作目标转轴的直线方程用点斜式来表示：

9.根据权利要求8所述基于视觉SLAM估计空间旋转非合作目标转轴的方法，其特征在于：所述采用奇异值分解求解X，具体过程为：

将矩阵A进行奇异值分解，写成A＝U∑V^T的形式，

其中，U为AA^T的特征向量组成的正交矩阵，∑为AA^T的特征值的平方根组成的对角阵，V为A^TA的特征向量组成的正交矩阵；T为转置；

所以采用奇异值分解求解X′转换为使||U∑V^TX′||最小，而||U∑V^TX′||＝||∑V^TX′||，||V^TX′||＝||X′||＝1；

设Y′＝V^TX′，问题转换成||∑Y′||最小，Y′＝[0 0 0…1]^T，再由X′＝VY′求解出X′，X′的值就是V的最后一列。

Images