一种涉及动力轮打滑修正的多传感器融合载具定位方法

技术领域

本发明属于定位算法领域，具体涉及一种涉及动力轮打滑修正的多传感器融合载具定位方法。

背景技术

移动载具的定位技术是无人车、无人机以及复杂系统编队控制中的关键技术。移动载具实现轨迹跟踪、路径规划、编队控制等应用需要依赖载具的准确位置信息。

基于电机编码器和陀螺仪数据可以通过轮胎转速和载具加速度估计出全向轮载具的当前的速度与方向，通过积分可以得到载具的位移，短时定位精度可达到毫米级。但是由于分辨率的限制和量测噪声的存在，位移数据在长时间的计算中会产生漂移误差，影响定位的准确度。

基于超宽带(UWB)的定位方案通过测量脉冲信号的传输时间来获取标签与基站的直线距离。通过一系列几何计算获得标签相对基站的位置。这种方法具有定位数据不漂移的优点。现有技术可以达到1kHz的一对一收发测距，但一般的定位往往存在多个基站与标签。这会导致数据更新速度的下降，实测使用Decawave公司的DWM1000开发的4基站4标签的定位系统的数据刷新速度为10Hz。此外，UWB定位的精度为分米级，在进行高速、精密的集群定位时，若完全依赖则精度难以保证。

发明内容

发明目的：针对上述存在的问题，本发明提出了一种涉及动力轮打滑修正的多传感器融合载具定位方法，将上述两种测量方式有效融合，提升总体定位精度，在数据融合的同时完成了对载具打滑扰动的估计，从而补偿打滑对定位数据的扰动。有效提高定位数据融合的精度。

上述的目的通过以下技术方案实现：

一种涉及动力轮打滑修正的多传感器融合载具定位方法，该方法包括如下步骤：

S1.前馈控制：使用前馈PID算法对每个电机的转速做闭环控制，基于编码器数据获得三个动力轮的转速ω

S2.超宽带定位：基于UWB定位系统获得全向轮载具的绝对位置坐标(x

S3.打滑修正：根据电机负载状态估计出每个动力轮的相对移速修正值ν

S4.数据融合：使用卡尔曼滤波对上述两种位置数据进行融合，得到融合结果x

所述的涉及动力轮打滑修正的多传感器融合载具定位方法，步骤S1中所述前馈控制的具体方法是：

S11.在全向轮载具的三个动力轮的电机轴上安装径向磁铁，使用正交霍尔传感器检测径向磁铁产生的脉冲，根据该脉冲频率计算径向磁铁转速，也即是电机转速；

S22.根据步骤S11中得到的电机转速得到动力轮外廓行进的速度，具体公式如下：

v

其中v

所述的涉及动力轮打滑修正的多传感器融合载具定位方法，步骤S2中所述超宽带定位的具体方法是：在全向轮载具上挂载的超宽带标签与周边的基站通讯并测距，通过最小二乘估计获得全向轮载具的绝对位置(x

所述的涉及动力轮打滑修正的多传感器融合载具定位方法，步骤S3中所述打滑修正的具体方法是：

S31.计算转速误差：设计前馈加PID电机转速控制算法，得到在前馈控制下控制的转速误差ω

ω

其中ω

S32.根据步骤S31得到的转速误差计算转矩误差T

转矩误差T

T

其中k

S33.根据步骤S32中得到的转矩误差T

当差值的绝对值小于等于正常波动阈值T

当差值的绝对值大于正常波动阈值阈值T

当差值的绝对值大于正常波动阈值阈值T

对于状态1，电机工作正常，未出现打滑现象，编码器反馈的转动距离与电机行驶距离一致，此时直接给出修正值：

ω

对于状态2，电机负载小于额定值，意味着电机与地面接触不良，可能出现了打滑，此时将电机转速设为ω

其中x

与下时刻超宽带定位返回的位置(x

此时修正值为

对于状态3，电机负载大于额定值，意味着电机重载，此时可能行驶到阻力较大区域，也可能是碰到障碍而无法继续行进，此时将电机转速设为ω

所述的涉及动力轮打滑修正的多传感器融合载具定位方法，步骤S4中所述数据融合的具体方法是：

S41.使用步骤S3中获得的电机转速修正值ω

其中[v

S42.再通过步骤S41得到的机体直角坐标系下的速度分量获得大地坐标系下的速度分量：

其中θ为机体坐标系与大地坐标系的偏角，也即是载具的航向角，[v'

由式(8)、式(9)可得：

S43.使用状态方程实现对当前状态的计算，即式(11)：

其中

S44.载具的平面坐标使用卡尔曼滤波方式融合UWB定位数据和编码器数据，迭代计算公式如下：

式(12.2)计算出式(12.1)先验估计的协方差矩阵，其中

式(12.3)计算出卡尔曼增益K

式(12.4)根据卡尔曼增益计算位置的最优估计数值，Z

式(12.5)计算得到式(12.4)t时刻滤波输出值的协方差矩阵P

根据全向轮载具实际状态，针对控制器代码优化，其迭代格式可简化表示为

其中

有益效果：

本发明利用了前馈控制网络的过程量实现对负载转矩的估计和检测，从而判断车轮的工作状态(是否打滑)，继而得到准确的动力轮外廓行进速度。基于该速度实现更为精确的位置估计，尤其提升了位置估计的鲁棒性。在载具动力轮堵转、打滑时，使用该算法能更加快速的辨识故障，给出准确的位置信息而无需增加额外的传感器。

附图说明

图1是全向轮载具示意图；

图2是前馈PID电机转速控制框图；

图3是动力轮电机转速与电压的函数图。

具体实施方式

下面结合附图，对所提出的一种涉及动力轮打滑修正的多传感器融合载具定位方法进行详细说明：

一种涉及动力轮打滑修正的多传感器融合载具定位方法，该方法包括如下步骤：

S1.前馈控制：使用前馈PID算法对每个电机的转速做闭环控制，基于编码器数据获得三个动力轮的转速ω

S2.超宽带定位：基于UWB定位系统获得全向轮载具的绝对位置坐标(x

S3.打滑修正：根据电机负载状态估计出每个动力轮的相对移速修正值ν

S4.数据融合：使用卡尔曼滤波对上述两种位置数据进行融合，得到融合结果x

所述的涉及动力轮打滑修正的多传感器融合载具定位方法，步骤S1中所述前馈控制的具体方法是：

S11.在全向轮载具的三个动力轮的电机轴上安装径向磁铁，使用正交霍尔传感器检测径向磁铁产生的脉冲，根据该脉冲频率计算径向磁铁转速，也即是电机转速；设计如图1控制框图的前馈加PID电机转速控制方法，其中U(ω)为正常行驶情况下动力轮电机转速与电机电压的函数关系。此函数需要在实验中测得。本实施例中描述的算法所使用的载具电机所对应的函数图像如图2所示，图2中横轴为动力轮电机的转速，单位为转每秒，纵轴为动力轮电机两端电压值，单位为伏特。

S22.根据步骤S11中得到的电机转速得到动力轮外廓行进的速度，具体公式如下：

v

其中v

所述的涉及动力轮打滑修正的多传感器融合载具定位方法，步骤S2中所述超宽带定位的具体方法是：在全向轮载具上挂载的超宽带标签与周边的基站通讯并测距，通过最小二乘估计获得全向轮载具的绝对位置(x

所述的涉及动力轮打滑修正的多传感器融合载具定位方法，步骤S3中所述打滑修正的具体方法是：

S31.计算转速误差：设计图1所示前馈加PID电机转速控制算法，得到在前馈控制下控制的转速误差ω

ω

其中ω

S32.根据步骤S31得到的转速误差计算转矩误差T

转矩误差T

T

其中k

S33.根据步骤S32中得到的转矩误差T

当差值的绝对值小于等于正常波动阈值T

当差值的绝对值大于正常波动阈值阈值T

当差值的绝对值大于正常波动阈值阈值T

对于状态1，电机工作正常，未出现打滑现象，编码器反馈的转动距离与电机行驶距离一致，此时直接给出修正值：

ω

对于状态2，电机负载小于额定值，意味着电机与地面接触不良，可能出现了打滑，此时将电机转速设为ω

其中x

与下时刻超宽带定位返回的位置(x

此时修正值为

对于状态3，电机负载大于额定值，意味着电机重载，此时可能行驶到阻力较大区域，也可能是碰到障碍而无法继续行进，此时将电机转速设为ω

所述的涉及动力轮打滑修正的多传感器融合载具定位方法，步骤S4中所述数据融合的具体方法是：

S41.使用步骤S3中获得的电机转速修正值ω

其中[v

S42.再通过步骤S41得到的机体直角坐标系下的速度分量获得大地坐标系下的速度分量：

其中θ为机体坐标系与大地坐标系的偏角，也即是载具的航向角，[v'

由式(8)、式(9)可得：

S43.使用状态方程实现对当前状态的计算，即式(11)：

其中

S44.载具的平面坐标使用卡尔曼滤波方式融合UWB定位数据和编码器数据，迭代计算公式如下：

式(12.1)为基于过去时刻状态及控制量对当前时刻的状态进行估计，其中

式(12.2)计算出式(12.1)先验估计的协方差矩阵，其中

式(12.3)计算出卡尔曼增益K

式(12.4)根据卡尔曼增益计算位置的最优估计数值，Z

式(12.5)计算得到式(12.4)t时刻滤波输出值的协方差矩阵P

根据全向轮载具实际状态，针对控制器代码优化，其迭代格式可简化表示为

其中

上述实施例仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和等同替换，这些对本发明权利要求进行改进和等同替换后的技术方案，均落入本发明的保护范围。