[发明专利]一种基于视觉和惯性导航组合的移动机器人定位方法

权利要求书

1.一种基于视觉和惯性导航组合的移动机器人定位方法，其特征在于，包含以下步骤：

S1：利用视觉传感器获取移动机器人所在的位置：建立外参数模型，外参数将相机坐标系与世界坐标系对应；

S2：建立惯性导航系统，利用视觉传感器进行速度、方向和重力测量及报告；

S2.1：利用互补滤波法在惯性导航系统中进行姿态角解算：对惯性导航坐标体系与世界坐标系构建方向余弦矩阵，然后求导并对微分方程进行计算，最终得到姿态角的值；

S2.2：惯性导航航位推算：将惯性导航坐标系下的加速度转换到世界坐标系，将转换后的加速度扣除重力加速度后单次积分得到速度，二次积分得到位移；

S2.3：对测量的加速度和角速度分别进行惯性导航误差分析处理；

S3：利用视觉传感器采集的信息与SLAM系统构建的云地图转换为栅格地图后进行路径规划。

2.根据权利要求1所述的移动机器人定位方法，其特征在于，所述外参数模型为：

式中，(x_c,y_c,z_c)代表相机坐标，(x_w,y_w,z_w)代表世界坐标，R代表旋转矩阵，t代表平移矩阵，m代表外参数的矩阵。

3.根据权利要求1所述的移动机器人定位方法，其特征在于，所述惯性导航系统采用MPU6050传感器来测量坐标系三轴上的角速度与加速度，所述MPU6050传感器包含三轴陀螺仪与三轴加速度计。

4.根据权利要求1所述的移动机器人定位方法，其特征在于，所述惯性导航航位推算中惯性导航的加速度a_b(t)＝(a_bx(t),a_by(t),a_bz(t))^T，世界坐标系下的加速度a_n(t)＝c_bna_b(t)，其中，a_bx(t)、a_by(t)、a_bz(t)分别表示的是加速度a_b(t)在惯性坐标系x、y、z轴上的分量，C_bn表示的是惯性坐标系至世界坐标系的旋转矩阵。

5.根据权利要求4所述的移动机器人定位方法，其特征在于，所述世界坐标系下的加速度a_n(t)扣除重力加速度后一次积分，计算出速度v_g(t)，即其中，v_g(0)、g_g(0)分别代表初始速度、重力加速度。

6.根据权利要求5所述的移动机器人定位方法，其特征在于，所述速度v_g(t)进行再次积分，得到位移s_g(t)，即其中，s_g(0)为初始位移。

7.根据权利要求1所述的移动机器人定位方法，其特征在于，所述S2.3中测量的加速度以三轴加速度计为误差分析对象，对其建模为a_m＝a+g+δb+w_a，其中，a、w_a、a_m、b、δ、g分别表示实际加速度、随机噪声、测量加速度、加速度偏移量、加速度偏移量系数、重力加速度。

8.根据权利要求1所述的移动机器人定位方法，其特征在于，所述S2.3中测量的角速度为误差分析对象，对其建模为w_m＝w+b_c+b_w+n，其中，b_c、b_w、w、n、w_m分别表示的是静态偏移量、动态偏移量、角速度真实值、随机噪声、角速度测量值。

9.根据权利要求1所述的移动机器人定位方法，其特征在于，S3中所述云地图向栅格地图转换，借助机器人操作系统向外发布构建的地图阶段与定位节点。

10.根据权利要求9所述的移动机器人定位方法，其特征在于，所述栅格地图在move-base框架内完成定位节点与地图的订阅操作，实现机器人的局部与全局路径规划。