CN112985426B - 一种用于二轮车的定位方法 - Google Patents

Abstract

本发明提供了一种用于二轮车的定位方法，包括以下步骤：S1：安装里程计、陀螺仪和角度传感器，并对里程计和角度传感器进行标定；S2：获取测量误差的标准差参数；S3：建立状态方程和观测方程；S4：测量二轮车运动过程中的运动参数；S5：处理测量结果。本发明能够一定程度上克服只使用陀螺仪带来的角度误差，特别是长时间测量的角度累积误差对定位精度和稳定性的影响。

Description

一种用于二轮车的定位方法

技术领域

本发明涉及室外定位导航的技术领域，更具体地，涉及一种用于二轮车的定位方法。

背景技术

目前，室外定位主要采用卫星定位导航技术，但该技术受很多外界条件的限制，特别是在城市复杂环境中，如空间密集的建筑物、较长的隧道或类似隧道的场景等情况下，卫星信号弱甚至完全接收不到信号，导致定位精度受损甚至完全没有定位数据。解决方法是在移动目标(通常是移动车辆)上安装辅助的定位传感器，其中以里程计和陀螺仪为主要定位传感器，其中里程计提供的是移动目标的位移和速度信息，陀螺仪提供的是移动目标的移动方向(角度)信息。在一些移动目标中由于驱动轮分别控制，可以通过分别安装在不同驱动轮上的里程计的测量结果估计移动目标的移动方向(角度)信息。

二轮车主要是指自行车和摩托车等车辆，它的特点是前轮导向，后轮驱动。以摩托车为例，除了卫星定位设备以外，出于成本考虑，通常安装的定位传感器为里程计和陀螺仪。陀螺仪用于测量二轮车移动方向，由于二轮车不具备多个驱动轮的特点，所以里程计只能测量位移和速度信息。通常里程计的误差来源包括后轮直径测量误差以及路面光滑程度造成的误差，陀螺仪的误差来源是零偏不稳定和测量角度漂移，且影响随着时间的增长而增大。里程计+陀螺仪方案中的角度误差是影响最终定位精度以及稳定性的主要误差源。

因此，现有技术中亟需一种能够克服长时间测量的角度累积误差对定位精度和稳定性的影响的技术方案。

发明内容

针对上述现有技术的不足，本发明提供了一种用于二轮车的定位方法。

为实现上述目的，本发明通过下述技术方案予以实现：

一种用于二轮车的定位方法，包括以下步骤：

S1：安装里程计、陀螺仪和角度传感器，并对里程计和角度传感器进行标定；

S2：获取测量误差的标准差参数；

S3：建立状态方程和观测方程；

S4：测量二轮车运动过程中的运动参数；

S5：处理测量结果。

所述里程计包括霍尔传感器和磁铁片；

安装里程计具体为：将磁铁片安装在二轮车后轮轮毂上，将霍尔传感器安装在二轮车后轮支架上；

对所述里程计进行标定具体为：测量二轮车后轮的直径。

安装陀螺仪具体为：将陀螺仪安装在二轮车车体上除前把和前轮外的位置上，且保持陀螺仪与地面水平，使陀螺仪的转动与二轮车车体转动保持一致。

安装角度传感器具体为：将角度传感器安装在二轮车前把上，其中，角度传感器的中心轴安装在二轮车前把的轴上；角度传感器的壳体安装在二轮车前把的连接体上；

对角度传感器进行标定具体包括以下子步骤：

S101：将二轮车摆正，并使前轮的着地点位于角度尺的原点，此时角度传感器的测量值为参考输出；

S102：将前轮偏转一定角度，此时角度尺的测量值为真实角度，此时角度传感器的测量值为测量电压；

S103：多次重复步骤S102，连续多次测量不同真实角度下的测量电压，根据测量电压和真实角度的数据进行拟合，得到角度校正曲线，其中，0度值和斜率为最终的校正参数。

步骤S2具体为：对二轮车进行测试，使二轮车进行一定时间的直线运动，并且使二轮车尽可能保持匀速，记录里程计、陀螺仪和角度传感器实时的测量值，进行多次测试后，统计计算得到测量值的标准差，即为测量误差的标准差参数，

步骤S3包括以下子步骤：

S301：构建状态方程：

状态变量为[Δx_k Δy_k ΔS_k θ_k Δθ_k]^T，其中，

Δx_k Δy_k：k时刻二轮车在X、Y轴的位置偏移量；

ΔS_k：k时刻二轮车整体的位置偏移量；

θ_k：k时刻的二轮车车体的运动方向对应的角度；

Δθ_k：k时刻的二轮车的运动方向对应的角度偏移量；

状态方程如下：

[Δx_k+1 Δy_k+1 ΔS_k+1 θ_k+1 Δθ_k+1]^T＝f([Δx_k Δy_k ΔS_k θ_k Δθ_k]^T) (1)

其中，

其中，

l_f和l_r为前轮和后轮到车辆重心的距离，δ_fk是二轮车前轮的偏转角，ω_s和ω_θ是位移测量误差的方差和角度测量误差的方差，状态转移矩阵为：

状态方程的误差方差确定为Q＝1×10^-2；

S302：构建观测方程：

观测变量为：里程计k+1时刻的测量结果、角度传感器k时刻的测量结果和陀螺仪k+1时刻的测量结果；

其中，与状态变量的关系如下：

其中，ΔS_k+1来自里程计k+1时刻的测量结果，β_k来自角度传感器k时刻的测量结果，θ_{gyro_k+1}和Δθ_{gyro_k+1}自陀螺仪k+1时刻的测量结果；观测矩阵为：

观测误差的标准差采用步骤S302获得的针对不同传感器的观测误差参数，即：R＝[里程计观测误差方差角度传感器观测误差方差陀螺仪观测误差方差]。

所述运动参数包括二轮车的位移、航向角、俯仰角和横滚角，以及二轮车前轮相对车体的偏转角度；

步骤S4具体包括：设定定时参数，按照定时参数周期性地从读取里程计、陀螺仪和角度传感器的测量值，其中，里程计的测量值为二轮车的位移，陀螺仪的测量值包括航向角、俯仰角和横滚角，角度传感器的测量值为二轮车前轮相对车体的偏转角度。

二轮车的位移的测量方法如下：设里程计安装的磁铁片个数为M，定时周期内霍尔传感器被磁铁片激发的次数为N，二轮车后轮直径为D，则定时周期内位移为：

二轮车前轮相对车体的偏转角度的测量方法如下：取航向角为车体运动角度，角度传感器的测量结果根据步骤S1中获得的校正参数进行校正处理，校正后的数据即为二轮车前轮相对车体的偏转角度。

步骤S5具体包括以下子步骤：

S501：状态预测：

根据k-1时刻的状态变量X(k-1)和k-1时刻的状态转移矩阵Φ(k,k-1)预测k时刻的状态变量；

S502：方差预测：

根据k-1时刻的方差P(k-1)和k-1时刻的状态转移矩阵Φ(k,k-1)以及状态方程误差方差Q预测k时刻的方差；

S503：增益计算：

K(k)＝P(k,k-1)H^T(k)[H(k)P(k,k-1)H^T(k)+R]^-1 (9)

根据k-1时刻的方差P(k-1)和k-1时刻的观测矩阵H(k)以及观测方程误差方差R预测k时刻的增益；

S504：观测值与预测值差值计算：

根据k时刻的状态预测值与k时刻的观测值求差，获得观测值与预测值的差值，其中，角度传感器的观测值是k-1时刻的观测值；

S505：最终滤波结果和最终滤波方差计算：

在状态预测值基础上计算最终滤波结果如下：

根据预测值

和增益K(k)以及第四步获得的差值D(k)计算最终的滤波结果；

在方差预测值基础上计算最终滤波结果如下：

根据方差预测值P(k,k-1)和增益K(k)以及状态转移矩阵H(k)计算最终的方差结果。

本发明与现有技术相比的有益效果是：

1.角度传感器与陀螺仪共同完成角度测量，能够一定程度上克服只使用陀螺仪带来的角度误差，特别是长时间测量的角度累积误差对定位精度和稳定性的影响。

2.采用卡尔曼滤波处理作为车辆定位中多传感器测量结果融合的算法时，突出估计角度的偏移值作为状态变量的作用，提高角度估计的精度和稳定性，并最终提高位置估计的精度和稳定性。

附图说明

图1是里程计、陀螺仪和角度传感器在二轮车上的安装位置示意图。

图2是角度传感器结构示意图。

图3是角度传感器在二轮车前把上的安装示意图。

图4是对角度传感器进行角度零度校正的过程的示意图。

图5是对角度传感器进行角度正向角度校正的过程的示意图。

图6是对角度传感器进行角度负向角度校正的过程的示意图。

图7是二轮车运动模型示意图。

图8是角度传感器角度校正曲线示意图。

图中：1-霍尔传感器，2-磁铁片，3-角度传感器，4-陀螺仪，5-中心轴，6-壳体。

具体实施方式

下面根据具体实施方式对本发明做进一步阐述。

如图1-8所示的用于二轮车的定位方法，包括以下步骤：

S1：安装里程计、陀螺仪4和角度传感器3，并对里程计和角度传感器3进行标定：

本实施例中使用六自由度陀螺仪，即三轴加速器+三轴陀螺仪。

角度传感器3如图2所示，其中心轴5能够在一定范围内转动，中心轴5相对外部壳体6的转动角度对应着角度传感器3输出电压值，角度传感器3与陀螺仪4共同完成角度测量，能够一定程度上克服只使用陀螺仪4带来的角度误差，特别是长时间测量的角度累积误差对定位精度和稳定性的影响。

里程计包括霍尔传感器1和磁铁片2；

安装里程计具体为：将磁铁片2安装在二轮车后轮轮毂上，磁铁片2数量根据位移精度的要求决定，位移精度高则磁铁片2数量多，反之位移精度低则磁铁片2数量少；将霍尔传感器1安装在二轮车后轮支架上，霍尔传感器1作用面与磁铁片2保持有效的短距离，以保证二轮车后轮转动时磁铁片2与霍尔传感器1能够有效激发。

对里程计进行标定具体为：测量二轮车后轮的直径。

安装陀螺仪4具体为：将陀螺仪4安装在二轮车车体上除前把和前轮外的位置上，且保持陀螺仪4与地面水平，使陀螺仪4的转动与二轮车车体转动保持一致。

安装角度传感器3具体为：将角度传感器3安装在二轮车前把上，与二轮车前轮转动保持一致，其中，角度传感器3的中心轴5安装在二轮车前把的轴上；角度传感器3的壳体6安装在二轮车前把的连接体上；

角度传感器3的安装方向与二轮车前轮的垂直方向并不重合，存在一定的夹角，所以角度传感器3测量的结果并不是前轮真实的偏转角度，因此需要进行校正。

对角度传感器3进行标定具体包括以下子步骤：

S101：将二轮车摆正，并使前轮的着地点位于角度尺的原点，标定此时角度传感器3的测量值为0°，前轮的着地点位于角度尺的原点。

S102：将前轮偏转一定角度，此时角度尺的测量值为真实角度，此时角度传感器3的测量值为测量电压；步骤S102中，将前轮向二轮车的正方向一侧偏转时，真实角度值为正；将前轮向二轮车的负方向一侧偏转时，真实角度值为负。

S103：多次重复步骤S102，连续多次测量不同真实角度下的测量电压，根据测量电压和真实角度的数据进行拟合，得到角度校正曲线，其中，0度值和斜率为最终的校正参数。

S2：获取测量误差的标准差参数；

步骤S2具体包括：对二轮车进行测试，为了保证测试过程的一致性，测试路径为直线，即二轮车在直线上运动，直线长度保持在20米以上，二轮车进行一定时间的直线运动，并且使二轮车尽可能保持匀速，速度约为1m/s，以获得足够的测试样本开展统计分析，记录里程计、陀螺仪4和角度传感器3实时的测量值，在直线运动中，由于二轮车前轮理论上没有发生偏转，因此，其测量结果应该为0°；由于20m的运动时间不长，陀螺仪4的角度误差漂移现象并不严重，其测量结果从理论上讲应该维持不变；在尽可能保持匀速的条件下，里程计每次的测量结果应该维持在一个稳定的数值，进行多次测试后，统计计算得到测量值的标准差，即为测量误差的标准差参数。

其中，里程计的测量值为车体在定时周期(如0.1s)内的位移，陀螺仪4的测量值为航向角、俯仰角和横滚角，角度传感器3的测量值为二轮车前轮相对车体的偏转角度。

运载体横轴与水平线之间的夹角叫横滚角，指导航***中用来标识目标的横向倾角，其值等于目标物体所在平面上与艏艉线垂直的线与其在水平面的投影间的夹角。横滚代表运载体绕纵轴的转动，绕纵轴轴向顺时针转动为正。

一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角。

俯仰角：机体坐标系X轴与水平面的夹角。当X轴的正半轴位于过坐标原点的水平面之上时，俯仰角为正，按习惯，俯仰角θ的范围为：-π/2≤θ≤π/2。

二轮车的位移的测量方法如下：设里程计安装的磁铁片2个数为M，定时周期内霍尔传感器1被磁铁片2激发的次数为N，二轮车后轮直径为D，则定时周期内位移为：

二轮车前轮相对车体的偏转角度的测量方法如下：陀螺仪4为水平安装，因而取航向角为车体运动角度，角度传感器3的测量结果根据步骤S1中获得的校正参数进行校正处理，校正后的数据即为二轮车前轮相对车体的偏转角度。

S3：建立状态方程和观测方程：

步骤S3包括以下子步骤：

S301：构建状态方程：

卡尔曼滤波是车辆定位设计中常用的解决多传感器测量结果融合的算法，涉及到的状态变量包括移动车辆的位置坐标以及车辆的移动方向如下：

[Δx_k Δy_k θ_k]^T

其中，Δx_k Δy_k：k时刻的X、Y轴的位置偏移量；

θ_k：k时刻的二轮车车体的运动方向对应的角度；

为了与测量方程中的里程计结果相对应，在状态变量中增加了位移增量ΔS_k。另外，由于累积效应，陀螺仪4的角度测量结果随着时间的变化而变化，反应出测量结果的不稳定，但是在短时间内其角度测量结果是相对稳定的，因此本发明将对应着二轮车车体运动方向的角度进行微分运算，即短时间内前后时刻角度值的差值(角度偏移值)也作为状态变量进行描述，最终的状态变量为：[Δx_k Δy_k ΔS_k θ_k Δθ_k]^T，其中，

Δx_k Δy_k：k时刻二轮车在X、Y轴的位置偏移量；

ΔS_k：k时刻二轮车整体的位置偏移量；

θ_k：k时刻的二轮车车体的运动方向对应的角度；

Δθ_k：k时刻的二轮车的运动方向对应的角度偏移量；

状态方程如下：

[Δx_k+1 Δy_k+1 ΔS_k+1 θ_k+1 Δθ_k+1]^T＝f([Δx_k Δy_k ΔS_k θ_k Δθ_k]^T) (1)

其中，

其中，

l_f和l_r为前轮和后轮到车辆重心的距离，δ_fk是二轮车前轮的偏转角，ω_s和ω_θ是位移测量误差的方差和角度测量误差的方差，状态转移矩阵为：

状态方程的误差方差确定为Q＝1×10^-2；

S302：构建观测方程：

观测变量为：里程计k+1时刻的测量结果、角度传感器3k时刻的测量结果和陀螺仪4k+1时刻的测量结果；

其中，与状态变量的关系如下：

其中，ΔS_k+1来自里程计k+1时刻的测量结果，β_k来自角度传感器3k时刻的测量结果，θ_{gyro_k+1}和Δθ_{gyro_k+1}自陀螺仪4k+1时刻的测量结果；观测矩阵为：

观测误差的标准差采用步骤S302获得的针对不同传感器的观测误差参数，即：R＝[里程计观测误差方差角度传感器3观测误差方差陀螺仪4观测误差方差]。

S4：测量二轮车运动过程中的运动参数：

运动参数包括二轮车的位移、航向角、俯仰角和横滚角，以及二轮车前轮相对车体的偏转角度；步骤S4具体包括：设定定时参数，按照定时参数周期性地从读取里程计、陀螺仪4和角度传感器3的测量值。

S5：处理测量结果：

步骤S5具体包括以下子步骤：

S501：状态预测：

根据k-1时刻的状态变量X(k-1)和k-1时刻的状态转移矩阵Φ(k,k-1)预测k时刻的状态变量；

S502：方差预测：

根据k-1时刻的方差P(k-1)和k-1时刻的状态转移矩阵Φ(k,k-1)以及状态方程误差方差Q预测k时刻的方差；

S503：增益计算：

K(k)＝P(k,k-1)H^T(k)[H(k)P(k,k-1)H^T(k)+R]^-1 (9)

根据k-1时刻的方差P(k-1)和k-1时刻的观测矩阵H(k)以及观测方程误差方差R预测k时刻的增益；

S504：观测值与预测值差值计算：

根据k时刻的状态预测值与k时刻的观测值求差，获得观测值与预测值的差值，其中，角度传感器3的观测值是k-1时刻的观测值；

S505：最终滤波结果和最终滤波方差计算：

在状态预测值基础上计算最终滤波结果如下：

根据预测值

和增益K(k)以及第四步获得的差值D(k)计算最终的滤波结果；

在方差预测值基础上计算最终滤波结果如下：

根据方差预测值P(k,k-1)和增益K(k)以及状态转移矩阵H(k)计算最终的方差结果。

以上所述的仅是本发明的优选实施方式，但本发明并不局限于上述的具体实施方式，对于本领域的普通技术人员来说，在不脱离本创造构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。

Claims (9)

1.一种用于二轮车的定位方法，其特征是，包括以下步骤：

S1：安装里程计、陀螺仪和角度传感器，并对里程计和角度传感器进行标定；

S2：获取里程计、陀螺仪和角度传感器的测量误差的标准差参数；

S3：建立状态方程和观测方程；

S4：测量二轮车运动过程中的运动参数；

S5：处理测量结果；

其中，步骤S3包括以下子步骤：

S301：构建状态方程：

状态变量为[Δx_k Δy_k ΔS_k θ_k Δθ_k]^T，其中，

Δx_k Δy_k：k时刻二轮车在X、Y轴的位置偏移量；

ΔS_k：k时刻二轮车整体的位置偏移量；

θ_k：k时刻的二轮车车体的运动方向对应的角度；

Δθ_k：k时刻的二轮车的运动方向对应的角度偏移量；

状态方程如下：

[Δx_k+1 Δy_k+1 ΔS_k+1 θ_k+1 Δθ_k+1]^T＝f([Δx_k Δy_k ΔS_k θ_k Δθ_k]^T) (1)

其中，

其中，

l_f和l_r为前轮和后轮到车辆重心的距离，δ_fk是二轮车前轮的偏转角，ω_s和ω_θ是位移测量误差的方差和角度测量误差的方差，状态转移矩阵为：

状态方程的误差方差确定为Q＝1×10^-2；

S302：构建观测方程：

观测变量为：里程计k+1时刻的测量结果、角度传感器k时刻的测量结果和陀螺仪k+1时刻的测量结果；

其中，与状态变量的关系如下：

其中，ΔS_k+1来自里程计k+1时刻的测量结果，β_k来自角度传感器k时刻的测量结果，θ_{gyro_k+1}和Δθ_{gyro_k+1}自陀螺仪k+1时刻的测量结果；观测矩阵为：

观测误差的标准差采用步骤S302获得的针对不同传感器的观测误差参数，即：R＝[里程计观测误差方差角度传感器观测误差方差陀螺仪观测误差方差]。

2.根据权利要求1所述的用于二轮车的定位方法，其特征是，所述里程计包括霍尔传感器和磁铁片；

安装里程计具体为：将磁铁片安装在二轮车后轮轮毂上，将霍尔传感器安装在二轮车后轮支架上；

对所述里程计进行标定具体为：测量二轮车后轮的直径。

3.根据权利要求1所述的用于二轮车的定位方法，其特征是，安装陀螺仪具体为：将陀螺仪安装在二轮车车体上除前把和前轮外的位置上，且保持陀螺仪与地面水平，使陀螺仪的转动与二轮车车体转动保持一致。

4.根据权利要求1所述的用于二轮车的定位方法，其特征是，安装角度传感器具体为：将角度传感器安装在二轮车前把上，其中，角度传感器的中心轴安装在二轮车前把的轴上；角度传感器的壳体安装在二轮车前把的连接体上；

对角度传感器进行标定具体包括以下子步骤：

S101：将二轮车摆正，并使前轮的着地点位于角度尺的原点，标定此时角度传感器的测量值为0°；

S102：将前轮偏转一定角度，此时角度尺的测量值为真实角度，记录此时角度传感器的测量电压；

S103：多次重复步骤S102，连续记录不同真实角度下的测量电压，根据测量电压和真实角度的数据进行拟合，得到角度校正曲线，其中，0度值和斜率为最终的校正参数。

5.根据权利要求4所述的用于二轮车的定位方法，其特征是，步骤S102中，将前轮向二轮车的正方向一侧偏转时，真实角度值为正；将前轮向二轮车的负方向一侧偏转时，真实角度值为负。

6.根据权利要求1所述的用于二轮车的定位方法，其特征是，步骤S2具体为：对二轮车进行测试，使二轮车进行一定时间的直线运动，并且使二轮车尽可能保持匀速，记录里程计、陀螺仪和角度传感器实时的测量值，进行多次测试后，统计计算得到测量值的标准差，即为测量误差的标准差参数，其中，里程计的测量值为二轮车车体的位移，陀螺仪的测量值为航向角、俯仰角和横滚角，角度传感器的测量值为二轮车前轮相对车体的偏转角度。

7.根据权利要求1所述的用于二轮车的定位方法，其特征是，所述运动参数包括二轮车的位移、航向角、俯仰角和横滚角，以及二轮车前轮相对车体的偏转角度；

步骤S4具体包括：设定定时参数，按照定时参数周期性地从读取里程计、陀螺仪和角度传感器的测量值，其中，里程计的测量值为二轮车车体的位移，陀螺仪的测量值包括航向角、俯仰角和横滚角，角度传感器的测量值为二轮车前轮相对车体的偏转角度。

8.根据权利要求7所述的用于二轮车的定位方法，其特征是，二轮车的位移的测量方法如下：设里程计安装的磁铁片个数为M，定时周期内霍尔传感器被磁铁片激发的次数为N，二轮车后轮直径为D，则定时周期内位移为：

二轮车前轮相对车体的偏转角度的测量方法如下：取航向角为车体运动角度，角度传感器的测量结果根据步骤S1中获得的校正参数进行校正处理，校正后的数据即为二轮车前轮相对车体的偏转角度。

9.根据权利要求1所述的用于二轮车的定位方法，其特征是，步骤S5具体包括以下子步骤：

S501：状态预测：

根据k-1时刻的状态变量X(k-1)和k-1时刻的状态转移矩阵Φ(k,k-1)预测k时刻的状态变量；

S502：方差预测：

根据k-1时刻的方差P(k-1)和k-1时刻的状态转移矩阵Φ(k,k-1)以及状态方程误差方差Q预测k时刻的方差；

S503：增益计算：

K(k)＝P(k,k-1)H^T(k)[H(k)P(k,k-1)H^T(k)+R]^-1 (9)

根据k-1时刻的方差P(k-1)和k-1时刻的观测矩阵H(k)以及观测方程误差方差R预测k时刻的增益；

S504：观测值与预测值差值计算：

根据k时刻的状态预测值与k时刻的观测值求差，获得观测值与预测值的差值，其中，角度传感器的观测值是k-1时刻的观测值；

S505：最终滤波结果和最终滤波方差计算：

在状态预测值基础上计算最终滤波结果如下：

根据预测值

和增益K(k)以及第四步获得的差值D(k)计算最终的滤波结果；

在方差预测值基础上计算最终滤波结果如下：

根据方差预测值P(k,k-1)和增益K(k)以及状态转移矩阵H(k)计算最终的方差结果。

Images