CN107560613A - 基于九轴惯性传感器的机器人室内轨迹跟踪系统及方法 - Google Patents
基于九轴惯性传感器的机器人室内轨迹跟踪系统及方法 Download PDFInfo
- Publication number
- CN107560613A CN107560613A CN201710697285.2A CN201710697285A CN107560613A CN 107560613 A CN107560613 A CN 107560613A CN 201710697285 A CN201710697285 A CN 201710697285A CN 107560613 A CN107560613 A CN 107560613A
- Authority
- CN
- China
- Prior art keywords
- msub
- robot
- axle
- coordinate system
- axis
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Landscapes
- Manipulator (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Gyroscopes (AREA)
Abstract
本发明公开一种基于九轴惯性传感器的机器人室内轨迹跟踪系统，包括可移动机器人，所述机器人上设置有依次相连接的传感器模块、传感器控制模块和上位机；所述机器人移动时，所述传感器模块会实时产生三轴加速度值、三轴角速度值以及三轴磁场强度值，所述传感器控制模块用于对所述传感器模块进行运行控制以及数据的收发工作，所述上位机用于对所获取的数据进行处理，以对所述机器人的移动轨迹进行跟踪。本发明不依赖于GPS信号，受环境因素影响小，可用于没有GPS信号环境下的机器人室内移动轨迹的跟踪与定位，并且具有较高的精度和实用价值。本发明还公开一种基于九轴惯性传感器的机器人室内轨迹跟踪系统的跟踪方法。
Description
技术领域
本发明涉及一种基于九轴惯性传感器的机器人室内轨迹跟踪系统及跟踪方法。
背景技术
正常的室外环境下，移动机器人上集成GPS传感器可以对设备所在位置进行定位，但在信号较弱的室内环境或没有GPS信号的情况下，其定位效果不理想或者失效。使用三轴加速度计和三轴陀螺仪组合的惯性测量单元(Inertial Measurement Unit,IMU)来进行室内定位虽可以实现对机器人轨迹的跟踪，但受到漂移和噪声的影响，其轨迹误差随着时间增长发散严重，导致重建后的轨迹与实际机器人移动轨迹偏离严重、精度低。
为了降低IMU的误差，目前常用的方法有使用激光雷达设备来辅助IMU进行室内定位与导航或者使用视觉传感器和IMU相结合等来进行导航和定位。这些方法虽然对定位的精度有所提高，但都需要提供外部环境的信息，在很多情况下，由于设备不足导致这些信息无法获得，并且这些设备的价格都比较昂贵，因此极大的限制了这些方法的应用。
发明内容
为了克服现有技术的不足，解决GPS导航技术在室内失效的情况，满足机器人室内运动轨迹跟踪的精确度，提供一种基于九轴惯性传感器的机器人室内轨迹跟踪系统及跟踪方法。
本发明采用由三轴加速度计、三轴陀螺仪和三轴磁力计组成的九轴惯性传感器作为惯性传感器模块。引入基于四元数的坐标系旋转关系描述方法，使用三轴加速度计和三轴磁力计的输出值来修正三轴陀螺仪的输出值，以获得精确的旋转关系。对获取的三轴加速度值进行坐标系转换以及去除重力处理，将处理后的三轴加速度值进行积分运算可获得机器人移动过程中的速度信息。采用一种静止时期(即零速点)判断方法，用于检测出机器人移动过程中的静止时期，结合判断出的静止时期对速度进行修正，对修正后的速度再次进行积分运算则可获得机器人移动过程中的位移信息。并且可以使用数据处理软件对获取的位移信息进行3D图像绘制，具有较好的可视化效果。
为了实现上述的目的，本发明采用如下技术方案予以实现。
一种基于九轴惯性传感器的机器人室内轨迹跟踪系统，包括可移动机器人，所述机器人上设置有依次相连接的传感器模块、传感器控制模块和上位机；所述机器人移动时，所述传感器模块会实时产生三轴加速度值、三轴角速度值以及三轴磁场强度值，所述传感器控制模块与传感器模块相连接，用于对所述传感器模块进行运行控制以及数据的收发工作，所述上位机与传感器控制模块相连接，用于对所获取的数据进行处理，以对所述机器人的移动轨迹进行跟踪。
所述传感器模块为整合了三轴加速度计、三轴陀螺仪和三轴磁力计的九轴惯性传感器，或整合三轴加速度、三轴陀螺仪的六轴惯性传感器加上一个单独的三轴磁力计以组成九轴惯性传感器，或使用单独封装的三轴加速度计、三轴陀螺仪和三轴磁力计，将它们的数据进行组合处理以达到九轴惯性传感器效果的传感器。
所述传感器控制模块为ARM处理器、单片机或微处理器。
所述机器人为可移动机器人，可将传感器模块、传感器控制模块以及上位机固定在机器人上，用于采集机器人移动过程中产生的数据并进行处理。
为了实现上述的目的，本发明采用如下另一技术方案予以实现。
一种基于九轴惯性传感器的机器人室内轨迹跟踪系统的跟踪方法，其步骤是：
(1)通过上位机获取机器人移动过程中产生的九轴惯性数据，并对数据进行预处理，消除数据中的噪声干扰；
(2)使用四元数法来描述传感器模块的输出数据由载体坐标系转换到导航坐标系之间的旋转矩阵；
(3)使用三轴加速度计和三轴磁力计输出值去修正三轴陀螺仪的输出值以获得更加准确的旋转矩阵；
(4)通过旋转矩阵将载体坐标系下的三轴加速度值转换到导航坐标系，对转换后的三轴加速度值进行积分运算，从而获得机器人移动过程中的速度信息；
(5)通过三轴加速度值判断机器人是否处于静止时期，即是否是零速度，利用这个静止时期对速度进行修正，对修正后的速度再次积分运算，则可获得机器人移动过程中的位移信息；
上述步骤(1)具体包括如下步骤：
(11)上位机以采样频率Fs读取九轴惯性传感器数据，读取的数据保存到csv文件中；
(12)将保存完成的csv文件导入上位机软件中，并对其中的三轴加速度值进行滤波预处理。
上述步骤(12)中，采用巴特沃兹低通滤波器对三轴加速度值进行滤波处理。
上述步骤(2)的具体步骤是：采用计算量小，算法简单的四元数法来描述传感器的输出数据由载体坐标系转换到导航坐标系之间的旋转矩阵：
第一步：通常在导航系统中选取三轴加速度计、三轴磁力计和三轴陀螺仪组成的惯性测量单元所在坐标系为载体坐标系b，该坐标系的原点为载体的重心，3个轴分别对应载体的横轴、纵轴和竖轴；选取地理坐标系为导航所参考坐标系，一般用n表示。使用四元数法可以很方便的表示n系至b系之间的转换关系；
式中，为n系至b系的旋转矩阵表示为：
第二步：采用四元数微分方程可以求解出旋转四元数；
其中表示四元数乘法，为载体坐标系b相对于导航坐标系n的三轴角速度。设采样周期为Δt，则四元数q的更新方程为:
上述步骤(2)中，通过四元数q的更新方程可以达到对旋转矩阵进行更新的目的。
上述步骤(3)中，引入Proportional Integral(PI)算法，通过计算三轴加速度计与三轴磁力计的输出值在两个坐标系下的转换误差去修正三轴陀螺仪的输出值，从而修正旋转矩阵中的四元数，以便获得更加准确的旋转矩阵。
上述步骤中的修正过程具体为：
设重力加速度向量在导航坐标系(n系)中的理论输出值为(上标表示单位化处理)，通过旋转矩阵变换后在载体坐标系(b系)下的值表示为同时载体坐标系中的传感器模块测得的重力加速度的值表示为对两个向量做叉积，则有误差矩阵：
其同理，设n系中磁场强度为经过旋转矩阵变换后在b系中的输出值为而载体坐标系中传感器模块测得的磁场强度为则有误差矩阵：
则总的转换误差为：
err＝erra+errm
设补偿后的三轴陀螺仪计算值记为gyro_new，PI算法中的比例增益记为kp，积分增益记为ki，则有：
gyro_new＝gyro+kp·err+ki·err·Δt
将修正后的三轴陀螺仪计算值代入步骤(2)中，则可对旋转四元数进行修正，从而对旋转矩阵进行修正。
上述步骤(4)具体步骤是：利用修正后的旋转矩阵转换三轴加速度值的坐标系，对转换坐标后的三轴加速度值进行积分处理，获得机器人移动过程中的速度信息：
第一步：将载体坐标系上的三轴加速度值通过前文方法获得修正后的旋转矩阵的逆变换矩阵转换到地理坐标系上，则有：
式中：an为地理坐标系下的三轴方向上的加速度输出值，ab为载体坐标系下三轴方向上的加速度输出值。为载体坐标系(b系)到地理坐标系(n系)的旋转矩阵，该旋转矩阵的四元数表示如下：
第二步：对an进行单位转换，去除重力影响后获得速度更新方程，去除积分漂移修正速度更新方程。求解步骤如下：
步骤①：将an的单位转换为m/s2，转换单位后的三轴加速度值输出记为acc，则有：
acc＝an*9.81
步骤②：去除Z轴固有的1g垂直重力分量影响，将acc中Z轴的加速度输出值记为accz，则有：
accz＝accz-9.81
步骤③：对去除重力影响后的三轴加速度值进行积分，获得速度更新方程，则有：
vel(t)＝vel(t-1)+acc(t)·Δt
式中，Δt为采样周期，acc(t)为t时刻的三轴加速度值，vel(t)为所求的t时刻的速度值。
上述步骤(5)中，通过计算三轴加速度值的矢量和，根据零速区间内加速度矢量的变化设定阈值，通过比较t时刻三轴加速度的矢量和模值与阈值的大小，判断该时刻是否为静止时期，利用这个静止时期对速度进行修正，对修正后的速度再次积分运算，则可获得机器人移动过程中的位移信息。
上述位移信息求解的具体过程是：
第一步：通过三轴加速度值判断静止时期。求解步骤如下：
设t时刻x、y、z三轴加速度值分别为accx(t)、accy(t)、accz(t)，那么t时刻的三轴加速度矢量和为：
设定一个最小阈值amin，当t时刻的三轴加速度矢量和模值为0或小于等于这个阈值时则认为此时机器人处于静止时期；用stationary表示静止时期，当满足静止时期检测标准时stationary为1，其他情况下stationary为0，那么则有：
第二步：计算静止时期速度不为0产生的积分漂移d。求解步骤如下：
d＝dr*dt
式中，dr为漂移速率。dt为静止时期的长度，即漂移时长。i和j分别为静止时期的开始与截止。velstationary(j-1)为静止前的一次采样速度。
第三步：利用积分漂移修正步骤(4)中所求的速度，修正后的t时刻的速度记为v(t)，则有：
上式中，当stationary为1即静止时期时此时的速度置为0，其他时期则根据更新公式计算速度的值。
第四步：对修正后的速度再次进行积分运算，则t时刻位移pos(t)更新方程：
pos(t)＝pos(t-1)+v(t)·Δt
采用上述方案后，本发明具有如下有益效果：
(1)本发明不依赖于GPS信号，受环境因素影响小，可用于没有GPS信号环境下的机器人轨迹跟踪与定位；
(2)本发明使用简单的九轴惯性传感器即可对机器人轨迹进行跟踪，实验设备简单；
(3)本发明使用的硬件成本低，具有较高实用价值；
附图说明
图1一种基于九轴惯性传感器的机器人室内轨迹跟踪系统的结构示意图；
图2一种基于九轴惯性传感器的机器人室内轨迹跟踪方法的总体流程示意图；
图3是计算旋转矩阵部分的流程图；
图4是对旋转矩阵进行修正部分流程图；
图5是计算速度更新方程部分流程图；
图6是对速度更新方程进行修正部分流程图；
图7是计算位移更新方程部分流程图。
具体实施方式
以下将结合附图，对本发明的技术方案进行详细说明。
参考图1，为本发明的一种基于九轴惯性传感器的机器人室内轨迹跟踪系统的结构示意图。该基于九轴惯性传感器的机器人室内轨迹跟踪系统，包括可移动机器人，传感器模块、传感器控制模块和上位机。其中传感器模块采用整合了三轴加速度计、三轴陀螺仪和三轴磁力计的九轴惯性传感器，或整合三轴加速度、三轴陀螺仪的六轴惯性传感器加上一个单独的三轴磁力计以组成九轴惯性传感器，或使用单独封装的三轴加速度计、三轴陀螺仪和三轴磁力计，传感器模块通过RS232串口通信方式与传感器控制模块进行数据传输，将传感器模块固定在机器人上，当机器人移动时，传感器模块会在x、y、z三个方向上实时产生加速度值、加速度值以及磁场强度值。
传感器控制模块采用ARM处理器、单片机或微处理器，将传感器控制模块的输入端通过串口与传感器模块输出端相连接，用于对传感器模块的工作模式进行配置，并接收传感器产生的数据，最后将接收到的数据通过USB传输到上位机。
上位机优先选用个人笔记本电脑，上位机中安装有对实验数据进行分析处理的软件，该软件优先选用MATLAB软件，通过该软件配合下面本文所提出的跟踪方法，可以达到对机器人移动轨迹跟踪的目的。
机器人可选用任何一款可移动的机器人，但优先选用美国Willow Garage公司出产的小型移动机器人turtlebot，该机器人小巧灵活，并且为上中下三层结构，下层为移动机器人底盘，中层和上层自带有可以载物的圆盘，三层之间通过支架支撑固定，可将传感器模块和传感器控制模块固定在机器人的中层圆盘上，而个笔记本则放在上层圆盘上。
本发明的一种基于九轴惯性传感器的机器人室内轨迹跟踪方法，参考图2，为本发明一种基于九轴惯性传感器的机器人室内轨迹跟踪方法的总体流程示意图；参考图3，为计算旋转矩阵部分的流程图；参考图4，为旋转矩阵进行修正部分流程图；参考图5，为计算速度更新方程部分流程图；参考图6，为速度更新方程进行修正部分流程图；参考图7，为计算位移更新方程部分流程图，具体包括如下步骤：
(1)通过上位机获取机器人移动过程中产生的九轴惯性数据，并对数据进行预处理，消除数据中的噪声干扰；
具体包括如下内容：
a)将九轴惯性传感单元固定在机器人上，上位机以采样频率Fs读取机器人移动过程中九轴惯性传感单元产生的数据，并将数据保存为csv文件。
b)使用MATLAB软件读取保存的csv文件，为了能够通过三轴加速度值检测出机器人移动过程中的静止时期，需要对三轴加速度值进行滤波预处理。为了能够实时观察到机器人所处的环境，控制机器人移动时速度介于正常人行走速度和跑步速度之间，而正常行走时人的步频处于1Hz至2.4Hz之间,跑步时步频最高为3.5Hz,为了能够精确的检测出机器人在移动过程中所有的静止时期，采用通带截止频率为3Hz,阻带截止频率为10Hz的巴特沃斯低通滤波器对三轴加速度值进行滤波处理，该滤波器的滤波窗口如图2所示。
(2)使用四元数法来描述传感器值由载体坐标系转换到导航坐标系之间的旋转矩阵；
两个坐标系之间的相对旋转关系可以使用四元数、欧拉角和方向余弦三种方式描述。由于欧拉角存在万向锁问题，方向余弦存在计算量过大的缺点，本发明采用计算量小，算法简单的四元数法来描述这种旋转关系。具体步骤如下：
第一步：通常在导航系统中选取三轴加速度计、三轴磁力计和三轴陀螺仪组成的惯性测量单元所在坐标系为载体坐标系b，该坐标系的原点为载体的重心，3个轴分别对应载体的横轴、纵轴和竖轴；选取地理坐标系为导航所参考坐标系，一般用n表示。使用四元数法可以很方便的表示n系至b系之间的转换关系；
式中，为n系至b系的旋转矩阵表示为：
第二步：采用四元数微分方程可以求解出旋转四元数；
其中表示四元数乘法，为载体坐标系b相对于导航坐标系n的三轴角速度。假设采样周期为Δt，那么四元数q的更新方程为:
通过四元数q的更新方程可以达到对旋转矩阵进行更新的目的。
(3)使用三轴加速度计和三轴磁力计的输出值去修正三轴陀螺仪的输出值以获得更加准确的旋转矩阵；
陀螺仪是一种常见的姿态传感器，其具有良好的动态性且不受运动中加速度变化的影响，常常被用在无人机、机器人等设备上以达到实时对姿态进行测量和控制，可是由于陀螺仪容易受噪声和温漂的影响，会存在积分累计误差。加速度计和磁力计虽动态性能不佳，对于外部干扰比较敏感、测量噪声大，但不存在累计误差，因此可以将它们之间的优点相结合。
对于一个大小和方向都已知的向量，将它由一个坐标系旋转到另外一个坐标系后理论上该向量的大小和方向是不变，但是由于这个旋转矩阵不是完美的会导致实际值和理论值之间存在偏差，使用这个偏差可以对四元数进行修正，从而也对旋转矩阵中的元素进行了修正，进而就会获得更加精确的旋转矩阵。
引入PI算法，通过计算三轴加速度计与三轴磁力计的输出值在两个坐标系下的转换误差去修正三轴陀螺仪的输出值，从而修正旋转矩阵中的四元数，以便获得更加准确的旋转矩阵。
具体修正步骤如下：
假设重力加速度向量在导航坐标系(n系)中的理论输出值为(上标表示单位化处理)，通过旋转矩阵变换后在载体坐标系(b系)下的值表示为同时载体坐标系中的三轴加速度计测得的重力加速度的值表示为假设准确，则但是由于存在误差所以使用该误差来修正对两个向量做叉积，则有误差矩阵：
假设n系中磁场强度为经过旋转矩阵变换后在b系中的输出值为而载体坐标系中三轴磁力计测得的磁场强度为则有误差矩阵：
则总的转换误差为：
err＝erra+errm (7)
设补偿后的三轴陀螺仪计算值记为gyro_new，PI算法中的比例增益记为kp，积分增益记为ki，则有：
gyro_new＝gyro+kp·err+ki·err·Δt (8)
将修正后的三轴陀螺仪计算值代入式(3)中，则可对旋转四元数进行修正，从而对旋转矩阵进行修正。
4)通过旋转矩阵将载体坐标系下的三轴加速度值转换到导航坐标系，对转换后的三轴加速度值进行积分运算，从而获得机器人移动过程中的速度信息。
具体步骤如下：
第一步：将载体坐标系上的三轴加速度值通过步骤(3)获得修正后的旋转矩阵的逆变换矩阵转换到地理坐标系上，则有：
式中：an为地理坐标系下的三轴方向上的加速度输出值，ab为载体坐标系下三轴方向上的加速度输出值。为载体坐标系(b系)到地理坐标系(n系)的旋转矩阵，该旋转矩阵的四元数表示为：
第二步：对an进行单位转换，去除重力影响后获得速度更新方程，去除积分漂移修正速度更新方程。求解步骤如下：
步骤①：将an的单位转换为m/s2，转换单位后的三轴加速度值输出记为acc，则有：
acc＝an*9.81 (11)
步骤②：去除Z轴固有的1g垂直重力分量影响，将acc中Z轴的加速度计输出值记为accz，则有：
accz＝accz-9.81 (12)
步骤③：对去除重力影响后的三轴加速度值进行积分，获得速度更新方程，则有：
vel(t)＝vel(t-1)+acc(t)·Δt (13)
式中，Δt为采样周期，acc(t)为t时刻的三轴加速度值，vel(t)为所求的t时刻的速度值。
(5)通过三轴加速度值判断机器人是否处于静止时期，即是否是零速度。利用这个静止时期对速度进行修正，对修正后的速度再次积分运算，则可获得机器人移动过程中的位移信息。
机器人在移动的过程中，由于受到障碍物影响等原因会出现短暂的静止状态来调整接下来的移动方向，以躲避障碍物。将九轴惯性传感器固定在机器人上，当机器人出现静止状态时，那个短暂的时期所具有速度的理论值应该为0，但是由于速度是通过该时期内对传感器模块的三轴加速度值进行积分运算求得，而传感器模块受自身漂移以及外部噪声影响会导致该时刻的三轴加速度值在0的附近进行波动，因此设定一个阈值作为判断的标准，已达到对机器人静止时期精确判断的目的。
通过计算三轴加速度值的矢量和，比较该时刻三轴加速度模值与阈值的大小，得到该时刻是否为静止时期。利用这个静止时期对速度进行修正，对修正后的速度再次积分运算，则可获得机器人移动过程中的位移信息。具体步骤如下：
第一步：通过三轴加速度值判断静止时期。求解步骤如下：
设t时刻x、y、z三轴加速度值分别为accx(t)、accy(t)、accz(t)，那么t时刻的三轴加速度值矢量和为：
设定一个最小阈值amin，当t时刻的三轴加速度值矢量和模值为0或小于等于这个阈值时则认为此时机器人处于静止时期；用stationary表示静止时期，当满足静止时期检测标准时stationary为1，其他情况下stationary为0，那么则有：
第二步：计算静止时期速度不为0产生的积分漂移d。求解步骤如下：
d＝dr*dt (16)
式中，dr为漂移速率。dt为静止时期的长度，即漂移时长。i和j分别为静止时期的开始与截止。velstationary(j-1)为静止前的一次采样速度。
第三步：利用积分漂移修正步骤(4)中所求的速度，修正后的t时刻的速度记为v(t)，则有：
上式中，当stationary为1即静止时期时此时的速度置为0，其他时期则根据更新公式计算速度的值。
第四步：对修正后的速度再次进行积分运算，则t时刻位移pos(t)更新方程：
pos(t)＝pos(t-1)+v(t)·Δt (19)
以上实施案例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。
Claims (10)
1.一种基于九轴惯性传感器的机器人室内轨迹跟踪系统，包括可移动机器人，其特征在于：所述机器人上设置有依次相连接的传感器模块、传感器控制模块和上位机；所述机器人移动时，所述传感器模块会实时产生三轴加速度值、三轴角速度值以及三轴磁场强度值，所述传感器控制模块与传感器模块相连接，用于对所述传感器模块进行运行控制以及数据的收发工作，所述上位机与传感器控制模块相连接，用于对所获取的数据进行处理，以对所述机器人的移动轨迹进行跟踪。
2.根据权利要求1所述的一种基于九轴惯性传感器的机器人室内轨迹跟踪系统，其特征在于：所述传感器模块为整合了三轴加速度计、三轴陀螺仪和三轴磁力计的九轴惯性传感器，或整合三轴加速度、三轴陀螺仪的六轴惯性传感器加上一个单独的三轴磁力计以组成九轴惯性传感器，或使用单独封装的三轴加速度计、三轴陀螺仪和三轴磁力计，将它们的数据进行组合处理以达到九轴惯性传感器效果的传感器。
3.根据权利要求1所述的一种基于九轴惯性传感器的机器人室内轨迹跟踪系统，其特征在于：所述传感器控制模块为ARM处理器、单片机或微处理器。
4.一种如权利要求1所述的基于九轴惯性传感器的机器人室内轨迹跟踪系统的跟踪方法，其特征在于，包括如下步骤：
(1)通过上位机获取机器人移动过程中产生的九轴惯性数据，并对数据进行预处理，消除数据中的噪声干扰；
(2)使用四元数法来描述传感器模块的输出值由载体坐标系转换到导航坐标系之间的旋转矩阵；
(3)使用三轴加速度值和三轴磁场强度值去修正三轴角速度值以获得更加准确的旋转矩阵；
(4)通过旋转矩阵将载体坐标系下的三轴加速度值转换到导航坐标系，对转换后的三轴加速度值进行积分运算，获得机器人移动过程中的速度信息；
(5)通过三轴加速度值判断机器人是否处于静止时期，即是否是零速度，利用这个静止时期对速度进行修正，对修正后的速度再次积分运算，则可获得机器人移动过程中的位移信息。
5.如权利要求4所述的一种基于九轴惯性传感器的机器人室内轨迹跟踪系统的跟踪方法，其特征在于：所述步骤(1)所述对数据进行预处理的具体方法包括如下步骤：
(11)所述上位机以采样频率Fs读取九轴惯性传感器数据，将读取的数据保存到csv文件中；
(12)将保存完成的csv文件导入上位机软件中，并对其中的三轴加速度值进行滤波处理。
6.如权利要求5所述的一种基于九轴惯性传感器的机器人室内轨迹跟踪系统的跟踪方法，其特征在于：步骤(12)中，所述滤波是采用巴特沃兹低通滤波器对三轴加速度值进行滤波处理。
7.如权利要求4所述的一种基于九轴惯性传感器的机器人室内轨迹跟踪系统的跟踪方法，其特征在于：所述步骤(2)中，所述四元数法是一种计算量小，算法简单的数学计算方法，该方法描述传感器模块的输出值由载体坐标系转换到导航坐标系之间的旋转矩阵；具体步骤是：
第一步：在导航系统中选取三轴加速度计、三轴磁力计和三轴陀螺仪组成的惯性测量单元所在坐标系为载体坐标系b，该坐标系的原点为载体的重心，3个轴分别对应载体的横轴、纵轴和竖轴；选取地理坐标系为导航所参考坐标系，用n表示，使用四元数法表示n系至b系之间的旋转关系；
第二步：采用四元数微分方程求解出旋转四元数更新方程；
第三步：采用四元数更新方程求解出旋转矩阵更新方程。
8.如权利要求4所述的一种基于九轴惯性传感器的机器人室内轨迹跟踪系统的跟踪方法，其特征在于：所述步骤(3)中，所述获得更加准确的旋转矩阵，是引入ProportionalIntegral(PI)算法，通过计算三轴加速度计与三轴磁力计的输出值在两个坐标系下的转换误差去修正三轴陀螺仪的输出值，从而修正旋转矩阵中的四元数，获得更加准确的旋转矩阵；具体步骤是：
第一步：使用三轴加速度计数据，利用如下公式求得误差矩阵erra；
<mrow>
<msub>
<mi>err</mi>
<mi>a</mi>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>a</mi>
<mi>y</mi>
</msub>
<msub>
<mi>v</mi>
<mi>z</mi>
</msub>
<mo>-</mo>
<msub>
<mi>a</mi>
<mi>z</mi>
</msub>
<msub>
<mi>v</mi>
<mi>y</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>a</mi>
<mi>z</mi>
</msub>
<msub>
<mi>v</mi>
<mi>x</mi>
</msub>
<mo>-</mo>
<msub>
<mi>a</mi>
<mi>x</mi>
</msub>
<msub>
<mi>v</mi>
<mi>z</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>a</mi>
<mi>x</mi>
</msub>
<msub>
<mi>v</mi>
<mi>y</mi>
</msub>
<mo>-</mo>
<msub>
<mi>a</mi>
<mi>y</mi>
</msub>
<msub>
<mi>v</mi>
<mi>x</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
式中，vx、vy、vz为导航坐标系(n系)下的重力加速度通过旋转矩阵转换至载体坐标系(b系)后在三轴的计算值，ax、ay、az为载体坐标系中的传感器模块测得的重力加速度在三轴的输出值；
第二步：使用三轴磁力计数据，利用如下公式求得误差矩阵errm；
<mrow>
<msub>
<mi>err</mi>
<mi>m</mi>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>c</mi>
<mi>y</mi>
</msub>
<msub>
<mi>m</mi>
<mi>z</mi>
</msub>
<mo>-</mo>
<msub>
<mi>c</mi>
<mi>z</mi>
</msub>
<msub>
<mi>m</mi>
<mi>y</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>c</mi>
<mi>z</mi>
</msub>
<msub>
<mi>m</mi>
<mi>x</mi>
</msub>
<mo>-</mo>
<msub>
<mi>c</mi>
<mi>x</mi>
</msub>
<msub>
<mi>m</mi>
<mi>z</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>c</mi>
<mi>x</mi>
</msub>
<msub>
<mi>m</mi>
<mi>y</mi>
</msub>
<mo>-</mo>
<msub>
<mi>c</mi>
<mi>y</mi>
</msub>
<msub>
<mi>m</mi>
<mi>x</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
式中，mx、my、mz为导航坐标系(n系)下的磁场强度值通过旋转矩阵转换至载体坐标系(b系)后在三轴的计算值，cx、cy、cz为载体坐标系中的传感器模块测得的磁场强度在三轴输出值；
第三步：对误差矩阵erra和errm使用矩阵加法运算，可求得总的误差err；
第四步：使用PI算法，利用如下公式，则可通过误差err去修正三轴陀螺仪的输出值；
gyro_new＝gyro+kp·err+ki·err·Δt
式中，gyro为三轴陀螺仪的输出值，gyro_new为修正后的三轴陀螺仪计算值，kp为PI算法中的比例增益记为，ki为PI算法中的积分增益，Δt为采样周期；
第五步：将修正后的三轴陀螺仪数据代入上述步骤(2)中，利用四元数法则可获得更加准确的旋转矩阵。
9.如权利要求4所述的一种基于九轴惯性传感器的机器人室内轨迹跟踪系统的跟踪方法，其特征在于：所述步骤(4)中，所述获得机器人移动过程中的速度信息的具体步骤是：
第一步：将载体坐标系上的三轴加速度值通过旋转矩阵转换到地理坐标系上；
第二步：对三轴加速度值进行单位转换，去除重力影响后获得速度更新方程，去除积分漂移后修正速度更新方程，求解步骤如下：
步骤①：将三轴加速度值的单位转换为m/s2；
步骤②：去除Z轴加速度值固有的1g垂直重力分量影响；
步骤③：对去除重力影响后的三轴加速度值进行积分，获得速度更新方程。
10.如权利要求4所述的一种基于九轴惯性传感器的机器人室内轨迹跟踪系统的跟踪方法，其特征在于：步骤(5)中所述获得机器人移动过程中的位移信息的具体步骤是：
第一步：设定阈值amin，利用三轴加速度值通过数值比较求取静止时期；
第二步：利用如下公式，计算静止时期速度不为0产生的积分漂移d；
d＝dr*dt
式中，dr为漂移速率，dt为静止时期的长度，即漂移时长；
第三步：将步骤(4)中所求的速度减去所求的积分漂移，则可求得修正后的速度更新方程；
第四步：对修正后的速度再次进行积分运算，则可求得位移更新方程。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710697285.2A CN107560613B (zh) | 2017-08-15 | 2017-08-15 | 基于九轴惯性传感器的机器人室内轨迹跟踪系统及方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710697285.2A CN107560613B (zh) | 2017-08-15 | 2017-08-15 | 基于九轴惯性传感器的机器人室内轨迹跟踪系统及方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107560613A true CN107560613A (zh) | 2018-01-09 |
| CN107560613B CN107560613B (zh) | 2020-03-31 |
Family
ID=60975409
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710697285.2A Active CN107560613B (zh) | 2017-08-15 | 2017-08-15 | 基于九轴惯性传感器的机器人室内轨迹跟踪系统及方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107560613B (zh) |
Cited By (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108362926A (zh) * | 2018-01-10 | 2018-08-03 | 云南电网有限责任公司电力科学研究院 | 一种电压等级识别方法及装置 |
| CN108627153A (zh) * | 2018-05-11 | 2018-10-09 | 南京大学 | 一种基于惯性传感器的刚体运动追踪系统及其工作方法 |
| CN109141410A (zh) * | 2018-07-25 | 2019-01-04 | 深圳市集大自动化有限公司 | Agv组合导航的多传感器融合定位方法 |
| CN109141433A (zh) * | 2018-09-20 | 2019-01-04 | 江阴市雷奥机器人技术有限公司 | 一种机器人室内定位系统及定位方法 |
| CN109540131A (zh) * | 2018-11-12 | 2019-03-29 | 西安文理学院 | 一种移动机器人组合惯性导航系统及方法 |
| CN109556601A (zh) * | 2018-10-29 | 2019-04-02 | 中国科学院电子学研究所 | 一种手势轨迹计算方法及手势轨迹采集装置 |
| CN110243361A (zh) * | 2018-03-08 | 2019-09-17 | 精工爱普生株式会社 | 惯性测量装置及移动体 |
| CN111504254A (zh) * | 2020-04-28 | 2020-08-07 | 深圳市云鼠科技开发有限公司 | 一种基于六轴陀螺仪的扫地机角度测算方法 |
| CN111722452A (zh) * | 2020-06-29 | 2020-09-29 | 烟台艾睿光电科技有限公司 | 一种红外产品的防抖稳像方法、装置、设备及存储介质 |
| US20210341944A1 (en) * | 2020-05-01 | 2021-11-04 | Kinze Manufacturing, Inc. | Gps location augmentation and outage playthrough |
| CN113741456A (zh) * | 2021-09-02 | 2021-12-03 | 乐聚(深圳)机器人技术有限公司 | 机器人路径目标点跟踪方法、装置、电子设备及存储介质 |
| CN113954065A (zh) * | 2021-09-28 | 2022-01-21 | 哈尔滨工业大学 | 一种基于惯导定位技术的机器人离线示教平台及其离线示教方法 |
| CN115372659A (zh) * | 2022-08-05 | 2022-11-22 | 东莞市汇能电子有限公司 | 一种通过三轴陀螺仪进行电池运动轨迹和速度计算的方法 |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103776451A (zh) * | 2014-03-04 | 2014-05-07 | 哈尔滨工业大学 | 一种基于mems的高精度三维姿态惯性测量系统以及测量方法 |
| CN104185050A (zh) * | 2014-07-30 | 2014-12-03 | 哈尔滨工业大学深圳研究生院 | 基于ott电视的智能遥控系统及其控制方法 |
| CN104197927A (zh) * | 2014-08-20 | 2014-12-10 | 江苏科技大学 | 水下结构检测机器人实时导航系统及方法 |
| CN104567931A (zh) * | 2015-01-14 | 2015-04-29 | 华侨大学 | 一种室内惯性导航定位的航向漂移误差消除方法 |
| CN105203098A (zh) * | 2015-10-13 | 2015-12-30 | 上海华测导航技术股份有限公司 | 基于九轴mems传感器的农业机械全姿态角更新方法 |
| CN106095113A (zh) * | 2016-06-27 | 2016-11-09 | 南京睿悦信息技术有限公司 | 一种九轴传感器融合的用户姿态测算和虚拟现实随动方法 |
-
2017
- 2017-08-15 CN CN201710697285.2A patent/CN107560613B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103776451A (zh) * | 2014-03-04 | 2014-05-07 | 哈尔滨工业大学 | 一种基于mems的高精度三维姿态惯性测量系统以及测量方法 |
| CN104185050A (zh) * | 2014-07-30 | 2014-12-03 | 哈尔滨工业大学深圳研究生院 | 基于ott电视的智能遥控系统及其控制方法 |
| CN104197927A (zh) * | 2014-08-20 | 2014-12-10 | 江苏科技大学 | 水下结构检测机器人实时导航系统及方法 |
| CN104567931A (zh) * | 2015-01-14 | 2015-04-29 | 华侨大学 | 一种室内惯性导航定位的航向漂移误差消除方法 |
| CN105203098A (zh) * | 2015-10-13 | 2015-12-30 | 上海华测导航技术股份有限公司 | 基于九轴mems传感器的农业机械全姿态角更新方法 |
| CN106095113A (zh) * | 2016-06-27 | 2016-11-09 | 南京睿悦信息技术有限公司 | 一种九轴传感器融合的用户姿态测算和虚拟现实随动方法 |
Cited By (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108362926A (zh) * | 2018-01-10 | 2018-08-03 | 云南电网有限责任公司电力科学研究院 | 一种电压等级识别方法及装置 |
| CN110243361A (zh) * | 2018-03-08 | 2019-09-17 | 精工爱普生株式会社 | 惯性测量装置及移动体 |
| CN110243361B (zh) * | 2018-03-08 | 2023-12-26 | 精工爱普生株式会社 | 惯性测量装置及移动体 |
| CN108627153A (zh) * | 2018-05-11 | 2018-10-09 | 南京大学 | 一种基于惯性传感器的刚体运动追踪系统及其工作方法 |
| CN109141410B (zh) * | 2018-07-25 | 2020-09-01 | 深圳市集大自动化有限公司 | Agv组合导航的多传感器融合定位方法 |
| CN109141410A (zh) * | 2018-07-25 | 2019-01-04 | 深圳市集大自动化有限公司 | Agv组合导航的多传感器融合定位方法 |
| CN109141433A (zh) * | 2018-09-20 | 2019-01-04 | 江阴市雷奥机器人技术有限公司 | 一种机器人室内定位系统及定位方法 |
| CN109556601A (zh) * | 2018-10-29 | 2019-04-02 | 中国科学院电子学研究所 | 一种手势轨迹计算方法及手势轨迹采集装置 |
| CN109556601B (zh) * | 2018-10-29 | 2020-08-28 | 中国科学院电子学研究所 | 一种手势轨迹计算方法及手势轨迹采集装置 |
| CN109540131A (zh) * | 2018-11-12 | 2019-03-29 | 西安文理学院 | 一种移动机器人组合惯性导航系统及方法 |
| CN111504254A (zh) * | 2020-04-28 | 2020-08-07 | 深圳市云鼠科技开发有限公司 | 一种基于六轴陀螺仪的扫地机角度测算方法 |
| US20210341944A1 (en) * | 2020-05-01 | 2021-11-04 | Kinze Manufacturing, Inc. | Gps location augmentation and outage playthrough |
| US11937530B2 (en) * | 2020-05-01 | 2024-03-26 | Kinze Manufacturing, Inc. | GPS location augmentation and outage playthrough |
| CN111722452A (zh) * | 2020-06-29 | 2020-09-29 | 烟台艾睿光电科技有限公司 | 一种红外产品的防抖稳像方法、装置、设备及存储介质 |
| CN113741456A (zh) * | 2021-09-02 | 2021-12-03 | 乐聚(深圳)机器人技术有限公司 | 机器人路径目标点跟踪方法、装置、电子设备及存储介质 |
| CN113954065A (zh) * | 2021-09-28 | 2022-01-21 | 哈尔滨工业大学 | 一种基于惯导定位技术的机器人离线示教平台及其离线示教方法 |
| CN115372659A (zh) * | 2022-08-05 | 2022-11-22 | 东莞市汇能电子有限公司 | 一种通过三轴陀螺仪进行电池运动轨迹和速度计算的方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107560613B (zh) | 2020-03-31 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107560613A (zh) | 基于九轴惯性传感器的机器人室内轨迹跟踪系统及方法 | |
| CN108362282B (zh) | 一种基于自适应零速区间调整的惯性行人定位方法 | |
| CN106767804B (zh) | 一种运动物体的多维数据测量装置及方法 | |
| CN105300379B (zh) | 一种基于加速度的卡尔曼滤波姿态估计方法及系统 | |
| CN106979780B (zh) | 一种无人车实时姿态测量方法 | |
| CN101706287B (zh) | 一种基于数字高通滤波的旋转捷联系统现场标定方法 | |
| Ojeda et al. | FLEXnav: Fuzzy logic expert rule-based position estimation for mobile robots on rugged terrain | |
| CN101514900B (zh) | 一种单轴旋转的捷联惯导系统初始对准方法 | |
| CN201561759U (zh) | 惯性姿态方位测量装置 | |
| CN110007354B (zh) | 无人机半航空瞬变电磁接收线圈飞行参数测量装置及方法 | |
| CN106500693A (zh) | 一种基于自适应扩展卡尔曼滤波的ahrs算法 | |
| CN106949889A (zh) | 针对行人导航的低成本mems/gps组合导航系统及方法 | |
| CN106153073B (zh) | 一种全姿态捷联惯导系统的非线性初始对准方法 | |
| CN105806363B (zh) | 基于srqkf的sins/dvl水下大失准角对准方法 | |
| CN1932444B (zh) | 适用于高速旋转体的姿态测量方法 | |
| CN109682377A (zh) | 一种基于动态步长梯度下降的姿态估计方法 | |
| CN108007477B (zh) | 一种基于正反向滤波的惯性行人定位系统误差抑制方法 | |
| CN109540135B (zh) | 水田拖拉机位姿检测和偏航角提取的方法及装置 | |
| CN108318038A (zh) | 一种四元数高斯粒子滤波移动机器人姿态解算方法 | |
| CN113340298B (zh) | 一种惯导和双天线gnss外参标定方法 | |
| CN106482746A (zh) | 一种用于混合式惯导系统的加速度计内杆臂标定与补偿方法 | |
| CN106885566A (zh) | 一种可穿戴式运动传感器及其抗磁场干扰的方法 | |
| CN103644910A (zh) | 基于分段rts平滑算法的个人自主导航系统定位方法 | |
| CN105929836A (zh) | 用于四旋翼飞行器的控制方法 | |
| CN110672095A (zh) | 一种基于微惯导的行人室内自主定位算法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20201023 Address after: 215500 No.13, Caotang Road, Changshu, Suzhou, Jiangsu Province Patentee after: Changshu intellectual property operation center Co.,Ltd. Address before: 212003, No. 2, Mengxi Road, Zhenjiang, Jiangsu Patentee before: JIANGSU University OF SCIENCE AND TECHNOLOGY |
|
| TR01 | Transfer of patent right | ||
| CP02 | Change in the address of a patent holder |
Address after: 215500 5th floor, building 4, 68 Lianfeng Road, Changfu street, Changshu City, Suzhou City, Jiangsu Province Patentee after: Changshu intellectual property operation center Co.,Ltd. Address before: No.13 caodang Road, Changshu City, Suzhou City, Jiangsu Province Patentee before: Changshu intellectual property operation center Co.,Ltd. |
|
| CP02 | Change in the address of a patent holder | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20210817 Address after: 215500 No. 33, Mogan Road, Mocheng street, Changshu City, Suzhou City, Jiangsu Province Patentee after: CHANGSHU CHANGJIANG STAINLESS STEEL MATERIAL Co.,Ltd. Address before: 215500 5th floor, building 4, 68 Lianfeng Road, Changfu street, Changshu City, Suzhou City, Jiangsu Province Patentee before: Changshu intellectual property operation center Co.,Ltd. |
|
| TR01 | Transfer of patent right |