CN111207758A - 一种基于加速度感应与磁感应的移动轨迹精确测量方法及其装置 - Google Patents

Abstract

本发明利用高精度的加速度感应、磁感应技术与精确的数学模型提供一种基于加速度感应与磁感应的移动轨迹精确测量方法及其装置，通过安装在车辆两后车轮的加速度模块、磁感应模块采集加速度感应、磁感应数据，利用数学模型计算出车轮转动角度，并通过车轮转动角度计算出车轮模块行驶距离，根据阿克曼转向几何，对车辆两个后车轮转弯时的过程进行建模，将车轴中心点的坐标偏移和偏向角变化量代入三角函数计算出车辆地面坐标系的位移(Δx,Δy)；相对于传统测量轨迹方法，本测量系统计算每个时间间隔内车辆后车轮轴的位置变化和角度变化，代表整台车辆的运动情况，数学模型要求的参数更少，测量或获取的方式更加简单，抗干扰能力强并且测量精度更高。

Description

一种基于加速度感应与磁感应的移动轨迹精确测量方法及其 装置

技术领域

本发明涉及一种利用传感器确定车辆位移、轨迹测量技术领域，尤其涉及一种基于加速度感应传感器与磁感应传感器的转动角度精确测量系统。

背景技术

精确定位技术，在过去的几十年取得了很大的进展，基本可以分为两类，一类是相对位置定位，从某个基准点出发，记录方向、距离进行定位，如惯性导航，测向测距等，另一类则为绝对位置（坐标）定位，如GPS，视觉定位，地形匹配，恒星定位；绝对位置定位，现在已经达到了较高的精度，尤其是基于GPS 的差分定位技术，现在可以达到厘米级别甚至毫米级别，但是民用领域的相对定位还在一个比较低的级别。

现有技术中对于转动角度的测量，多采用基于霍尔效应的脉冲计数装置，或者是与转轴联动的分度盘和旋转编码解码器方式，或者使用交流供电的自整角机：

脉冲计数装置广泛应用于车辆行程采集，在车轮上固定霍尔元件，通过采集车轮转动圈数，获取车辆行驶里程，此方法的最小计量单位为车轮转动一圈的周长，精度不高。

分度盘和旋转编码解码器，多用于精密伺服电机转动角度控制，通过给定脉冲控制电机转动角度，并通过分度盘、解码器获知电机当前角度。此方法成本较高，对使用环境限制严格，使用交流供电的自整角机在控制系统中得到了广泛应用，此方法成本较高，体积大，同时一般要求外部提供交流电源，对使用环境限制严格。

发明内容

本发明的目的，就是克服现有技术的不足，利用加速度模块、磁传感模块及由此建立的数学模型提供一种高精度、低误差的移动轨迹精确测量方法。

为了达到上述目的，本发明提供了一种基于加速度感应与磁感应的移动轨迹精确测量方法，包括以下步骤：

s1、在车轮圆心位置设置用于采集左手坐标系的加速度数据的加速度模块，利用加速度模块采集到的加速度数据

,

，计算出车辆两个车后轮的车轮转动角度α1、α2及车轮转动角度变化量Δα1、Δα2并计算出车辆移动轨迹；

s2 、将车轮转动角度变化量Δα1、Δα2分别和车轮模块的半径D代入公式，计算出车轮模块行驶距离Δs1、Δs2，通过车轮移动距离Δs1、Δs2计算出车辆移动轨迹；

s3、根据车轮移动圆弧外侧曲线L、车轮移动圆弧内侧曲线R、车轴长度W计算出内圆半径r；

s4、将内圆半径r代入公式计算出t到t+1时刻的弧度角φ；

s5、对弧度角φ进行角度制转换得到车辆偏向角变化量Δθ；

s6、以t时刻的车轴中心点为原点，建立前进方向为y轴正方向，从移动圆弧外侧车轮模块指向移动圆弧内侧车轮模块的方向为x轴正方向的坐标系，命名为车辆坐标系；

s7、将偏向角变化量Δθ、车轴长度W、内圆半径r代入公式计算出车轴中心点的坐标偏移d；

s8、将t时刻的偏向角θ(t)、t到t+1时刻的车轴中心点的坐标偏移d和偏向角变化量Δθ代入三角函数解出t到t+1时刻车辆在地面坐标系中的位移Δx、Δy；

s9、利用车辆偏向角变化量Δθ、位移Δx和位移Δy计算出车辆t+1时刻在地面坐标系上的位置x(t+1)、y(t+1)和偏向角θ(t+1)；

s10、利用车辆在地面坐标系上的位置x(t)、y(t)、θ(t)和x(t+1)、y(t+1)、θ(t+1)绘制出车辆移动轨迹。

所述车轮转动角度变化量Δα1、Δα2的计算方法为：

根据受力平衡建立方程计算t时刻车轮转动角度α1(t)、α2(t)、t+1时刻车轮转动角度α1(t+1)、α2(t+1)，再计算出车轮转动角度变化量Δα1、Δα2:

Δα1=α1(t+1)-α1(t)

Δα2=α2(t+1)-α2(t)

所述车轮模块行驶距离Δs1、Δs2的计算方法为：

Δs1=Δα1*D*

Δs2=Δα2*D*

。

所述车轮移动内圆半径r的计算方法：

所述车轮在t+1时刻移动构成圆弧的弧度角φ的计算方法为

所述弧度角φ换算成车辆偏向角变化量Δθ的计算方法为：

所述t+1时刻的车轴中心点的坐标偏移d的计算方法为：

所述t+1时刻车辆的地面坐标系位移Δx、Δy的计算方法为

所述新的车辆偏向角θ(t+1)=θ(t)+Δθ。

所述步骤s1至s5中的车轮转动角度α1 、车轮模块行驶距离Δs1和车辆偏向角变化量Δθ可用于测量单个车轮的移动轨迹。

一种基于加速度感应与磁感应的移动轨迹精确测量装置，包括：设置于车轮圆心位置的传感器、存储有可被执行用于实现如权利要求1至5中任一项所述方法的存储器；用于传输用于实现如权利要求1至5中任一项所述方法的数据传输模块；以及用于执行如权利要求1至5中任一项所述方法的数据处理器；用于融合如权利要求1至5中任一项所述方法数据的数据融合模块；

所述传感器包括：

加速度模块，用于检测物体在载体坐标系中三个轴的加速度信号并输出；

磁感应模块，用于检测地磁场的强度和方向信息并输出。

本发明的有益效果是：利用高精度的加速度感应、磁感应技术与精确的数学模型提供一种基于加速度感应与磁感应的移动轨迹精确测量方法及其装置，通过安装在车辆两个后车轮的加速度模块、磁感应模块采集加速度感应、磁感应数据，利用数学模型计算出车轮转动角度，并通过车轮转动角度计算出车轮模块行驶距离，根据阿克曼转向几何，对车辆两个后车轮转弯时的过程进行建模，将车轴中心点的坐标偏移和偏向角变化量代入三角函数计算出车辆地面坐标系的位移(Δx, Δy)；相对于传统测量轨迹方法，本测量系统计算每个时间间隔内车辆后车轮轴的位置变化和角度变化，代表整台车辆的运动情况，数学模型要求的参数更少，测量或获取的方式更加简单，抗干扰能力强并且测量精度更高。

附图说明

为了易于说明，本发明由下述的较佳实施例及附图作以详细描述。

图1是本发明所述一种基于加速度感应与磁感应的移动轨迹精确测量方法及其装置的车辆移动距离计算流程图；

图2是本发明所述一种基于加速度感应与磁感应的移动轨迹精确测量方法及其装置的移动轨迹计算流程图；

图3是本发明所述一种基于加速度感应与磁感应的移动轨迹精确测量方法及其装置的车轮移动示意图；

图4是本发明所述一种基于加速度感应与磁感应的移动轨迹精确测量方法及其装置的车辆两后车轮测量平面移动轨迹原理图；

图5是本发明所述一种基于加速度感应与磁感应的移动轨迹精确测量方法及其装置的地面坐标系中车辆两后车轮的移动轨迹示意图；

图6是本发明所述一种基于加速度感应与磁感应的移动轨迹精确测量方法及其装置的车辆四轮测量移动轨迹原理图;

图7是本发明所述一种基于加速度感应与磁感应的移动轨迹精确测量方法及其装置的齿轮受地磁力侧视示意图；

图8是本发明所述一种基于加速度感应与磁感应的移动轨迹精确测量方法及其装置的齿轮受地磁力俯视示意图；

图9 是本发明所述一种基于加速度感应与磁感应的移动轨迹精确测量方法及其装置的实验车辆轨迹图；

图10 是本发明所述一种基于加速度感应与磁感应的移动轨迹精确测量方法及其装置的磁感应模块安装示意图；

图11是是本发明所述一种基于加速度感应与磁感应的移动轨迹精确测量方法及其装置的模块示意图。

具体实施方式

下面参照附图对本发明所述的一种基于加速度感应与磁感应的移动轨迹精确测量方法及其装置做进一步说明:

实施例1：一种基于加速度感应与磁感应的移动轨迹精确测量方法，包括以下步骤：

s1、在车轮圆心位置设置用于采集左手坐标系的加速度数据的加速度模块，利用加速度模块采集到的加速度数据

,

，计算出车辆两个车后轮的车轮转动角度α1、α2及车轮转动角度变化量Δα1、Δα2并计算出车辆移动轨迹；

s2 、将车轮转动角度变化量Δα1、Δα2分别和车轮模块的半径D代入公式，计算出车轮模块行驶距离Δs1、Δs2，通过车轮移动距离Δs1、Δs2计算出车辆移动轨迹；

s3、根据车轮移动圆弧外侧曲线L、车轮移动圆弧内侧曲线R、车轴长度W计算出内圆半径r；

s4、将内圆半径r代入公式计算出t时刻到t+1时刻的弧度角φ；

s5、对弧度角φ进行角度制转换得到车辆偏向角变化量Δθ；

s6、以t时刻的车轴中心点为原点，建立前进方向为y轴正方向，从移动圆弧外侧车轮模块指向移动圆弧内侧车轮模块的方向为x轴正方向的坐标系，命名为车辆坐标系；

s7、将偏向角变化量Δθ、车轴长度W、内圆半径r代入公式计算出车轴中心点的坐标偏移d；

s8、将t时刻的偏向角θ(t)、t到t+1时刻的车轴中心点的坐标偏移d和偏向角变化量Δθ代入三角函数解出t到t+1时刻车辆在地面坐标系中的位移Δx、Δy；

s9、利用车辆偏向角变化量Δθ、位移Δx和位移Δy计算出车辆t+1时刻在地面坐标系上的位置x(t+1)、y(t+1)和偏向角θ(t+1);

s10、利用车辆在地面坐标系上的位置x(t)、y(t)、θ(t)和x(t+1)、y(t+1)、θ(t+1)绘制出车辆移动轨迹。

如图1、3所示，在理想情况下，当车轮转动一定角度时，可以从受力变化情况，推导出车轮转动角度，进而计算出车轮行驶距离；实际应用中，不可能将传感器完美放置在车轮的几何中心上，所以传感器会受到径向的离心力产生加速度，因向心加速度的数值相比较加速度较小，另在行驶过程中方向是周期性变化，在特定的形式环境(低速匀速)中产生的影响可以被一个周期内的同个因素所抵消，可忽略向心加速度，只考虑恒受到加速度以及可能存在的横向的车速变化的加速度；根据受力平衡建立方程计算t时刻车轮转动角度α1(t)、α2(t)、t+1时刻车轮转动角度α1(t+1)、α2(t+1)，再计算出车轮转动角度变化量Δα1、Δα2:

Δα1=α1(t+1)- α1(t)

Δα2=α2(t+1)- α2(t)

所述车轮模块行驶距离Δs1、Δs2的计算方法为：

。

如图2、4所示，当车辆在行驶过程中，如果某个较短时间间隔内，两个车轮行进的距离相同，那么车辆是直线行驶；如果其中一个车轮的行进距离大于另外一个车轮，那么意味着车辆在进行转弯。根据阿克曼转向几何，对车辆两个后车轮转弯时的过程进行建模,通过车辆两个车轮转动角度变化得到的两个车轮各自的行进距离Δs1、Δs2，根据转弯时车辆两个后车轮的轨迹是两个同心圆上的同个角所对的圆弧，通过已知的车辆两个车轮之间的间距也就是车轴的长度，可以建立起方程：

所述车轮移动内圆半径r的计算方法：

；

所述车轮在t+1时刻移动构成圆弧的弧度角φ的计算方法为：

；

所述弧度角φ换算成车辆偏向角Δθ的计算方法为：

。

如图5所示，

表示t时刻的速度矢量，

表示t+1时刻的速度矢量，将t+1时刻的车轴中心点的坐标偏移d和偏向角Δθ代入三角函数解出t+1时刻车辆地面坐标系的位移Δx、Δy和新的车辆偏向角,所述t+1时刻的车轴中心点的坐标偏移d的计算方法为：

；

所述t+1时刻车辆的地面坐标系位移Δx、Δy的计算方法为：

所述新的车辆偏向角θ(t+1)= θ(t)+Δθ。

所述步骤s1至s5中的车轮转动角度α1 、车轮模块行驶距离Δs1和车辆偏向角变化量Δθ可用于测量单个车轮的移动轨迹。

一种基于加速度感应与磁感应的移动轨迹精确测量装置，包括：设置于车轮圆心位置的传感器、存储有可被执行用于实现如权利要求1至5中任一项所述方法的存储器；用于传输用于实现如权利要求1至5中任一项所述方法的数据传输模块；以及用于执行如权利要求1至5中任一项所述方法的数据处理器；用于融合如权利要求1至5中任一项所述方法数据的数据融合模块；

所述传感器包括：

加速度模块，用于检测物体在载体坐标系中三个轴的加速度信号并输出；

磁感应模块，用于检测地磁场的强度和方向信息并输出。

实施例2：如图6所示，车辆四轮建模的原理和两轮相同，根据阿克曼转向几何，在转弯时每个车轮轨迹的圆弧圆心都落在同一点下标用1表示前车轮，2表示后车轮，L表示左轮R表示右轮，H表示前后同侧车轮之间的轴距，W表示后车轮之间的轴距。每对车轮实际上都可以建立起关于

的方程，取两前车轮为一组、两个后车轮为一组，共两组建立方程组，具体如下：

解上述两个方程之后各得到

，可以取均值来减小其中某个车轮潜在的误差带来的影响，计算到

之后，测量轨迹的算法和两个车轮测量移动轨迹轨迹步骤一致的；

四轮模型的优势还体现相比二轮模型，可以判断出更高维的运动变化，例如当车辆行驶过程中遇到上坡，通过车轮的转动情况可以发现前车轮一致地减速变慢，而在直线前进前后车轮转速相同，或者是转弯时前车轮的半径要大于后车轮时，所以前车轮的速度一般都是要大于后车轮的。

实施例3

如图7所示，本发明还可以用于水平转动齿轮或车轮转速测量，在一些无法使用加速度的场景中，例如水平转动的马达电机或者齿轮，则可以参照本发明所述的测量方法，使用磁感应模块捕捉水平转动过程中的磁场分量变化，计算出转动角度进而得到转速情况，附图7中

表示水平方向地磁方向，

表示垂直方向地磁方向；地磁场和重力势场存在于地球上每个角落，引力的方向是垂直朝下指向地心，而地磁场的方向比较特殊，既不是完全平行于地面也不是从地理南极指向地理北极，而是存在磁偏角。测量水平转动马达电机转子或者齿轮概要的步骤为，首先校准测量装置上的地磁方向，校准之后将能够捕捉地磁信号的传感装置安装在马达电机转子或者齿轮中心位置，根据装置的地磁三轴分量来计算马达电机转子或齿轮的实时转动角度，和前一个时刻的转动角度对比即可得出瞬时转速，计算x轴正向和地磁水平分量的转动角度

的公式为：

。

实施例4：

以两个车轮为例，将车轮模块1，车轮模块2 共两个，其中车轮模块1包含加速度模块1，数据传输模块1，数据处理器1，车轮模块2 包括加速度模块2，数据传输模块2，数据处理器2。将它们分别安装在车轮的中心位置，加速度模块采集车轮转动角度数据，经过数据传输模块传递到数据处理模块，在数据处理模块中进行角度到距离的计算，得到单个车轮在微小时间如50ms 前进的距离。

数据处理模块1，数据处理模块1 将数据汇总到数据融合模块1，在其中将两个车轮的前进距离进行融合，得到二维平面的轨迹，融合之后的轨迹图形如图9所示，实验用的车辆车轮半径为35cm，轮距为180-修正值cm(因轮胎与地面有一定的接触面积，经过实验此处的修正值为13)，传感器记录数据的周期是50ms即每秒20组数据。

行驶的路径模仿的是从停车位中向前行驶，左转至90度左右，然后倒车回到原来的位置。接收到带有时间标签的数据之后，使用插值方法来补全丢失的部分数据，以及矫正接收装置之间的时间差异，将两个车轮的数据对齐，之后按照以上两轮轨迹建模的方法计算绘制，得到如图9所示的轨迹图，从轨迹中可以看到计算结果和行驶的轨迹基本重合。

实施例5：

如图11所示，在条件允许时，也可在安装有磁感应模块的车轮之外，布设人造磁场，例如在车轮外框布设两颗永磁体，建立人造磁场，所述永磁体的磁场强度应远大于地磁强度，如达到

奥，所述人造磁场还可安装在车轮任意位置，或者磁感应模块安装在车轮任意位置，通过建立人造磁场可在地磁被屏蔽，或有电磁干扰的情况下准确定位;使用此方法时，人造磁场可以固定于轮上，此时传感器固定于板边;人造磁场也可以固定于板边，此时传感器固定于轮上; 使用人造磁场的优点在于，可以避开地磁扰动带来的影响;磁体与传感器的分离设计，也为安装实施带来了便利。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种基于加速度感应与磁感应的移动轨迹精确测量方法，其特征在于：包括以下步骤：

s1、在车轮圆心位置设置用于采集左手坐标系的加速度数据的加速度模块，或者在车轮任意位置设置磁感应模块，利用加速度模块采集到的加速度数据(v_x1,v_y1),( v_x2,v_y2)，计算出车辆两个车后轮的车轮转动角度α1、α2及车轮转动角度变化量Δα1、Δα2并计算出车辆移动轨迹；

s2、将车轮转动角度变化量Δα1、Δα2分别和车轮模块的半径D代入公式，计算出车轮模块行驶距离Δs1、Δs2，通过车轮移动距离Δs1、Δs2计算出车辆移动轨迹；

s3、根据车轮移动圆弧外侧曲线L、车轮移动圆弧内侧曲线R、车轴长度W计算出内圆半径r；

s4、将内圆半径r代入公式计算出t到t+1时刻的弧度角φ；

s5、对弧度角φ进行角度制转换得到车辆偏向角变化量Δθ；

s6、以t时刻的车轴中心点为原点，建立前进方向为y轴正方向，从移动圆弧外侧车轮模块指向移动圆弧内侧车轮模块的方向为x轴正方向的坐标系，命名为车辆坐标系；

s7、将偏向角变化量Δθ、车轴长度W、内圆半径r代入公式计算出车轴中心点的坐标偏移d；

s8、将t时刻的偏向角θ(t)、t到t+1时刻的车轴中心点的坐标偏移d和偏向角变化量Δθ代入三角函数解出t到t+1时刻车辆在地面坐标系中的位移Δx、Δy；

s9、利用车辆偏向角变化量Δθ、位移Δx和位移Δy计算出车辆t+1时刻在地面坐标系上的位置x(t+1)、y(t+1)和偏向角θ(t+1)；

s10、利用车辆在地面坐标系上的位置x(t)、y(t)、θ(t)和x(t+1)、y(t+1)、θ(t+1)绘制出车辆移动轨迹。

2.根据权利要求1所述的一种基于加速度感应与磁感应的移动轨迹精确测量方法，其特征在于，所述车轮转动角度变化量Δα1、Δα2的计算方法为：

根据受力平衡建立方程计算t时刻车轮转动角度α1(t)、α2(t)、t+1时刻车轮转动角度α1(t+1)、α2(t+1)，再计算出车轮转动角度变化量Δα1、Δα2:

Δα1=α1(t+1)-α1(t)

Δα2=α2(t+1)-α2(t)。

3.根据权利要求1所述的一种基于加速度感应与磁感应的移动轨迹精确测量方法，其特征在于，所述车轮模块行驶距离Δs1、Δs2的计算方法为：

。

4.根据权利要求1所述的一种基于加速度感应与磁感应的移动轨迹精确测量方法，其特征在于，所述车轮移动内圆半径r的计算方法：

所述车轮在t+1时刻移动构成圆弧的弧度角φ的计算方法为

所述弧度角φ换算成车辆偏向角变化量Δθ的计算方法为

。

5.根据权利要求1所述的一种基于加速度感应与磁感应的移动轨迹精确测量方法，其特征在于，所述t+1时刻的车轴中心点的坐标偏移d的计算方法为：

所述t+1时刻车辆的地面坐标系位移Δx、Δy的计算方法为

所述新的车辆偏向角为θ(t+1)= θ(t)+Δθ。

6.根据权利要求1所述的一种基于加速度感应与磁感应的移动轨迹精确测量方法，其特征在于，所述步骤s1至s5中的车轮转动角度α1 、车轮模块行驶距离Δs1和车辆偏向角变化量Δθ可用于测量单个车轮的移动轨迹。

7.一种基于加速度感应与磁感应的移动轨迹精确测量装置，其特征在于，包括：设置于车轮圆心位置的传感器、存储有可被执行用于实现如权利要求1至5中任一项所述方法的存储器；用于传输用于实现如权利要求1至5中任一项所述方法的数据传输模块；以及用于执行如权利要求1至5中任一项所述方法的数据处理器；用于融合如权利要求1至5中任一项所述方法数据的数据融合模块；

所述传感器包括：

加速度模块，用于检测物体在载体坐标系中三个轴的加速度信号并输出；

磁感应模块，用于检测磁场的强度和方向信息并输出。

Images