[发明专利]基于九轴惯性传感器的机器人室内轨迹跟踪系统及方法

权利要求书

1.一种基于九轴惯性传感器的机器人室内轨迹跟踪系统的跟踪方法，基于的跟踪系统包括可移动机器人，所述机器人上设置有依次相连接的传感器模块、传感器控制模块和上位机；所述机器人移动时，所述传感器模块会实时产生三轴加速度值、三轴角速度值以及三轴磁场强度值，所述传感器控制模块与传感器模块相连接，用于对所述传感器模块进行运行控制以及数据的收发工作，所述上位机与传感器控制模块相连接，用于对所获取的数据进行处理，以对所述机器人的移动轨迹进行跟踪；其中，所述传感器模块为整合了三轴加速度计、三轴陀螺仪和三轴磁力计的九轴惯性传感器，或整合三轴加速度、三轴陀螺仪的六轴惯性传感器加上一个单独的三轴磁力计以组成九轴惯性传感器，或使用单独封装的三轴加速度计、三轴陀螺仪和三轴磁力计，将它们的数据进行组合处理以达到九轴惯性传感器效果的传感器；所述传感器控制模块为ARM处理器、单片机或微处理器；其特征在于，所述的跟踪方法包括如下步骤：

(1)通过上位机获取机器人移动过程中产生的九轴惯性数据，并对数据进行预处理，消除数据中的噪声干扰；

(2)使用四元数法来描述传感器模块的输出值由载体坐标系转换到导航坐标系之间的旋转矩阵；

(3)使用三轴加速度值和三轴磁场强度值去修正三轴角速度值以获得更加准确的旋转矩阵；

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

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

其中，步骤(1)所述对数据进行预处理的具体方法包括如下步骤：

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

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

步骤(2)所述四元数法是一种计算量小，算法简单的数学计算方法，该方法描述传感器模块的输出值由载体坐标系转换到导航坐标系之间的旋转矩阵；具体步骤是：

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

第二步：采用四元数微分方程求解出旋转四元数更新方程；

第三步：采用四元数更新方程求解出旋转矩阵更新方程；

步骤(3)所述获得更加准确的旋转矩阵，是引入Proportional Integral(PI)算法，通过计算三轴加速度计与三轴磁力计的输出值在两个坐标系下的转换误差去修正三轴陀螺仪的输出值，从而修正旋转矩阵中的四元数，获得更加准确的旋转矩阵；具体步骤是：

第一步：使用三轴加速度计数据，利用如下公式求得误差矩阵err_a；

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

第二步：使用三轴磁力计数据，利用如下公式求得误差矩阵err_m；

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

第三步：对误差矩阵err_a和err_m使用矩阵加法运算，可求得总的误差err；

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

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

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

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

步骤(4)所述获得机器人移动过程中的速度信息的具体步骤是：

第一步：将载体坐标系上的三轴加速度值通过旋转矩阵转换到地理坐标系上；

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

步骤①：将三轴加速度值的单位转换为m/s²；

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

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

步骤(5)所述获得机器人移动过程中的位移信息的具体步骤是：

第一步：通过三轴加速度值判断静止时期，求解步骤如下：

设t时刻x、y、z三轴加速度值分别为accx(t)、accy(t)、accz(t)，那么t时刻的三轴加速度矢量和为：

设定一个最小阈值a_min，当t时刻的三轴加速度矢量和模值为0或小于等于这个阈值时则认为此时机器人处于静止时期；用stationary表示静止时期，当满足静止时期检测标准时stationary为1，其他情况下stationary为0，那么则有：

第二步：计算静止时期速度不为0产生的积分漂移d，求解步骤如下：

d＝dr*dt

式中，dr为漂移速率；dt为静止时期的长度，即漂移时长，i和j分别为静止时期的开始与截止，vel_stationary(j-1)为静止前的一次采样速度；

第三步：利用积分漂移修正步骤(4)中所求的速度，修正后的t时刻的速度记为v(t)，则有：

上式中，当stationary为1即静止时期时此时的速度置为0，其他时期则根据更新公式计算速度的值；

第四步：对修正后的速度再次进行积分运算，则t时刻位移pos(t)更新方程：

pos(t)＝pos(t-1)+v(t)·Δt。