[发明专利]基于单目视觉SLAM及深度不确定性分析的云台控制方法

权利要求书

1.一种基于单目视觉SLAM及深度不确定性分析的云台控制方法，由三自由度云台配合单目相机实现，以ORB-SLAM开源系统为主体框架。包括下列步骤：

1)确定路标点在重建过程中，受单位二维像素扰动影响所引起的三维位置偏差：

其中，Dist_base表示相邻两帧间的基线长度，Dist_est表示初步估计的路标深度值，角度α、β分别表示路标点与相邻两帧构成的位移向量与基线的夹角，Δβ表示单位二维像素扰动引起的角度偏差；

2)计算路标点的深度不确定性初值；

3)当一个路标点被多个图像观测到时，需要进行不确定性的融合，即实时更新当前图像观测到的路标点的深度不确定性：

其中，δ₁和δ₂分别代表两次对路标点的不确定性计算，δ_fuse代表更新结果；

4)建立并更新的路标点深度不确定性；

5)计算使“跟踪已知路标”与“探索未知区域”二者达到平衡的最优云台朝向:

构建相机的有效观测区域判别函数h(T，P)，用以判断相机在某一姿态T是否可以观测到路标点P：

其中，ε为单位阶跃函数，当函数内部大于0时，输出为1；否则输出为0；向量为路标点相对于相机中心的位移向量，为沿相机坐标系朝向的单位向量；代表向量与之间的夹角，由余弦定理获得；当路标点位于相机有效观测区域内时，判别函数h(T，P)结果为1；反之为0；

结合路标点的深度不确定性，构建如下优化问题：

式(4)为目标函数，式(5)为约束条件，优化变量为相机姿态矩阵T，求解使目标函数取最小值、且满足约束条件的相机姿态矩阵T，即可获得下一时刻的云台控制量；其中，N为环境地图中路标点总数，P_i为第i个路标点，T_k为相机当前时刻位姿，为路标点P_i在当前时刻的深度不确定性；ρ为平衡参数，用以调节“跟踪已知路标”与“探索未知区域”在控制决策中的影响力；a为度量参数；R为3×3的旋转矩阵，t为3×1的平移向量，t_k为当前时刻相机的平移向量，为机器人行进速度，Δt为云台控制周期。

2.根据权利要求1所述的方法，其特征在于，a＝1/1000。