[发明专利]一种机器人室内定位系统及定位方法

权利要求书

1.一种机器人室内定位系统，包括可移动的机器人，依次相连设置在所述机器人上的惯性传感器，传感器控制模块和上位机，其特征在于：所述上位机还连接有设置在机器人上的视觉传感器；当所述机器人移动时，所述惯性传感器实时产生三轴加速度值、三轴角速度值以及三轴磁场强度值，所述视觉传感器实时采集运动过程中的场景图像；所述传感器控制模块与惯性传感器相连接，用于对惯性传感器进行运行控制以及数据的收发工作，所述上位机与传感器控制模块相连接，用于对所获取的数据进行处理，所述上位机与视觉传感器连接通信，用于获取环境图像信息并对信息进行处理，所述惯性传感器与视觉传感器用于对机器人的移动轨迹进行定位与跟踪；其中所述惯性传感器为整合了三轴加速度计、三轴陀螺仪和三轴磁力计的九轴惯性传感器，或整合三轴加速度、三轴陀螺仪的六轴惯性传感器加上一个单独的三轴磁力计以组成九轴惯性传感器，或使用单独封装的三轴加速度计、三轴陀螺仪和三轴磁力计，将它们的数据进行组合处理以达到九轴惯性传感器的效果的惯性传感器；所述传感器控制模块为ARM处理器、单片机或微处理器；所述视觉传感器为单目相机、双目相机、RGB-D相机或者其他可以获取场景图像的视觉设备。

2.一种如权利要求1所述的机器人室内定位系统的定位方法，包括如下步骤：

步骤1、通过上位机获取机器人移动过程中产生的九轴惯性数据，并对数据进行预处理，消除数据中的噪声干扰；具体方法是：

步骤1-1、所述上位机以采样频率F_s读取九轴惯性传感器数据，将读取的数据保存到csv文件中；

步骤1-2、将保存完成的csv文件导入上位机软件中，并对其中的三轴加速度值采用巴特沃兹低通滤波器进行滤波处理；

步骤2、使用四元数法描述传感器模块的输出值由载体坐标系转换到导航坐标系之间的旋转矩阵；具体方法是：

步骤2-1、在导航系统中选取三轴加速度计、三轴磁力计和三轴陀螺仪组成的惯性测量单元所在坐标系为载体坐标系b，该坐标系的原点为载体的重心，3个轴分别对应载体的横轴、纵轴和竖轴；选取地理坐标系为导航所参考坐标系，用n表示，使用四元数法表示n系至b系之间的旋转关系；

步骤2-2、采用四元数微分方程求解出旋转四元数更新方程；

步骤2-3、采用四元数更新方程求解出旋转矩阵更新方程；

步骤3、使用三轴加速度值和三轴磁场强度值修正三轴角速度值以获得更加准确的旋转矩阵；具体方法是：

步骤3-1、使用三轴加速度计数据，利用如下公式求得误差矩阵err_a；

式中，v_x、v_y、v_z为导航坐标系(n系)下的重力加速度通过旋转矩阵转换至载体坐标系(b系)后在三轴的计算值，a_x、a_y、a_z为载体坐标系中的传感器模块测得的重力加速度在三轴的输出值；

步骤3-2、使用三轴磁力计数据，利用如下公式求得误差矩阵err_m；

式中，m_x、m_y、m_z为导航坐标系(n系)下的磁场强度值通过旋转矩阵转换至载体坐标系(b系)后在三轴的计算值，c_x、c_y、c_z为载体坐标系中的传感器模块测得的磁场强度在三轴输出值；

步骤3-3、对误差矩阵err_a和err_m使用矩阵加法运算，可求得总的误差err；

步骤3-4、使用PI算法，利用如下公式，则可通过误差err去修正三轴陀螺仪的输出值；

gyro_new＝gyro+k_p·err+k_i·err·Δt

式中，gyro为三轴陀螺仪的输出值，gyro_new为修正后的三轴陀螺仪计算值，k_p为PI算法中的比例增益记为，k_i为PI算法中的积分增益，Δt为采样周期；

步骤3-5、将修正后的三轴陀螺仪数据代入上述步骤(2)中，利用四元数法则可获得更加准确的旋转矩阵；

步骤4、通过旋转矩阵将载体坐标系下的三轴加速度值转换到导航坐标系，对转换后的三轴加速度值进行积分运算，获得机器人移动过程中的速度信息；具体方法是：

步骤4-1、将载体坐标系上的三轴加速度值通过旋转矩阵转换到地理坐标系上；

步骤4-2、对三轴加速度值进行单位转换，去除重力影响后获得速度更新方程，去除积分漂移后修正速度更新方程，求解步骤如下：

步骤4-2-1、将三轴加速度值的单位转换为m/s²；

步骤4-2-2去除Z轴加速度值固有的1g垂直重力分量影响；

步骤4-2-3对去除重力影响后的三轴加速度值进行积分，获得速度更新方程；

步骤5、通过三轴加速度值判断机器人是否处于静止时期，即是否是零速度，利用这个静止时期对速度进行修正，对修正后的速度再次积分运算，则可获得机器人移动过程中的位移信息；具体方法是：

步骤5-1、设定阈值a_min，利用三轴加速度值通过数值比较求取静止时期；

步骤5-2、利用如下公式，计算静止时期速度不为0产生的积分漂移d；

d＝dr*dt

式中，dr为漂移速率，dt为静止时期的长度，即漂移时长。

步骤5-4、将步骤4中所求的速度减去所求的积分漂移，则可求得修正后的速度更新方程；

步骤5-5、对修正后的速度再次进行积分运算，则可求得位移更新方程；

其特征在于还包括：

步骤6、对相机进行预处理，获取相机参数用于图像校正；

步骤7、提取校正后图像中的特征点，并进行特征匹配与去除外点；

步骤8、使用匹配后的特征点，通过特征点之间的几何关系解算出相机运动变换关系；

步骤9、通过获取到的运动变换关系构建位姿图；

步骤10、求解构建的位姿图优化问题，修正相机运动位姿，并重建场景地图；

步骤11、对惯性数据和视觉数据进行融合处理以完成对机器人的精确定位。