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

Abstract

本发明公开了一种涉及动力轮打滑修正的多传感器融合载具定位方法，该方法包括如下步骤：S1.前馈控制：使用前馈PID算法对每个电机的转速做闭环控制，基于编码器数据获得三个动力轮的转速，根据3个动力轮的尺寸得到相应的动力轮的相对移速；S2.超宽带定位：基于UWB定位***获得全向轮载具的绝对位置坐标；S3.打滑修正：根据电机负载状态估计出每个动力轮的相对移速修正值，基于模型解算得到位置，基于状态方程解出x_car、y_car、θ_car；S4.数据融合：使用卡尔曼滤波对上述两种位置数据进行融合，得到融合结果x_KF、y_KF、θ_KF。本发明可有效提高定位数据融合的精度。

Description

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

技术领域

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

背景技术

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

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

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

发明内容

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

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

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

S1.前馈控制：使用前馈PID算法对每个电机的转速做闭环控制，基于编码器数据获得三个动力轮的转速ω₁、ω₂、ω₃，根据3个动力轮的尺寸得到相应的动力轮的相对移速；

S2.超宽带定位：基于UWB定位***获得全向轮载具的绝对位置坐标(x_UWB,y_UWB)；

S3.打滑修正：根据电机负载状态估计出每个动力轮的相对移速修正值ν_{X_corr}，(X＝1,2,3)，基于模型解算得到位置大地坐标系下载具的横纵移速以及航向转速v_{x_car}、v_{y_car}、ω_car基于状态方程解出大地坐标系下载具的横纵坐标以及航向角度x_car、y_car、θ_car；

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

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

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

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

v_X＝k_rDω_X(X＝1,2,3) (1)

其中v_X(X＝1,2,3)为动力轮的相对移速，ω_X(X＝1,2,3)为动力轮电机转速，k_r为电机减速比，D为动力轮的直径。

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

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

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

ω_err＝ω_exp-ω_out (2)

其中ω_exp为期望转速，ω_out为实际转速，ω_err为在前馈控制下控制转速的误差；

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

转矩误差T_err与转速误差ω_err近似线性相关，将转速误差ω_err经过低通滤波后得到的ω_{err_LPF}消除了噪声影响，此时通过近似线性公式计算出转矩误差T_err，具体公式如下：

T_err＝k_wω_{err_LPF} (3)

其中k_w为线性化系数，转矩误差T_err即为电机负载偏离正常情况的数值；

S33.根据步骤S32中得到的转矩误差T_err，对电机的运行状态做分类并进行修正：

当差值的绝对值小于等于正常波动阈值T_thres时，记电机处于状态1；

当差值的绝对值大于正常波动阈值阈值T_thres且差值为正时，记电机处于状态2；

当差值的绝对值大于正常波动阈值阈值T_thres且差值为负时，记电机处于状态3；

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

ω_{X_corr}＝ω_X，(X＝1,2,3) (4)

对于状态2，电机负载小于额定值，意味着电机与地面接触不良，可能出现了打滑，此时将电机转速设为ω_X和0经过式(1)得到轮子外廓相对移速后分别代入式(5)计算估计下次的坐标得到两组坐标(x_ω,y_ω)、(x₀,y₀)：

其中x_t、y_t为当前时刻的横、纵坐标，x_t+1、y_t+1为下一时刻的横、纵坐标，t_Δ为两次计算时间的间隔，将电机转速设为ω_X代入计算得到的结果x_t+1、y_t+1分别记为x_ω、y_ω，将电机转速设为0代入计算得到的结果x_t+1、y_t+1分别记为x₀、y₀，

与下时刻超宽带定位返回的位置(x_UWB，y_UWB)分别计算其欧氏距离d_ω、d₀，公式如下：

此时修正值为

对于状态3，电机负载大于额定值，意味着电机重载，此时可能行驶到阻力较大区域，也可能是碰到障碍而无法继续行进，此时将电机转速设为ω_X和0经过式(1)得到轮子外廓相对移速后分别带入式(5)计算估计下次的坐标得到(x_ω,y_ω)、(x₀,y₀)，与下时刻超宽带定位返回的位置分别计算其欧氏距离d_ω、d₀，此时修正值参照式(7)得到。

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

S41.使用步骤S3中获得的电机转速修正值ω_{X_corr}(X＝1,2,3)，根据式(1)计算动力轮外廓行进速度修正值v_{X_corr}(X＝1,2,3),得到三个动力轮外廓行进速度后，对其进行变换，得到机体直角坐标系下的速度分量：

其中[v₁ v₂ v₃]^T表示三个动力轮外廓行进速度修正值，r为载具动力轮到载具中心的距离，[v_x v_y ω_z]^T中三个元素分别为载具机体直角坐标系下的横、纵速度分量以及航向轴的转速；

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

其中θ为机体坐标系与大地坐标系的偏角，也即是载具的航向角，[v'_x v'_y ω'_z]^T中三个元素分别为大地直角坐标系下的横、纵速度分量以及航向轴的转速，其中M^T表示矩阵M的转置；

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

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

其中

为t-1时刻卡尔曼滤波后的位置；

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

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

为t时刻估计值的协方差矩阵，P_t-1为t-1时刻滤波输出值的协方差矩阵，Q为式(12.1)计算引入误差的协方差矩阵，A^T表示矩阵A的转置；

式(12.3)计算出卡尔曼增益K_t，H为UWB定位数据转换到编码器数据转换矩阵，R为UWB定位数据误差的协方差矩阵，H^T表示UWB定位数据转换到编码器数据转换矩阵H的转置；

式(12.4)根据卡尔曼增益计算位置的最优估计数值，Z_t为t时刻UWB定位数据，

为为t时刻的卡尔曼估计值；

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

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

其中

为t时刻卡尔曼滤波的位置估计结果。

有益效果：

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

附图说明

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

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

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

具体实施方式

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

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

S1.前馈控制：使用前馈PID算法对每个电机的转速做闭环控制，基于编码器数据获得三个动力轮的转速ω₁、ω₂、ω₃，根据3个动力轮的尺寸得到相应的动力轮的相对移速ν₁、ν₂、ν₃；

S2.超宽带定位：基于UWB定位***获得全向轮载具的绝对位置坐标(x_UWB,y_UWB)；

S3.打滑修正：根据电机负载状态估计出每个动力轮的相对移速修正值ν_{X_corr}，(X＝1,2,3)，基于模型解算得到位置大地坐标系下载具的横纵移速以及航向转速v_{x_car}、v_{y_car}、ω_car基于状态方程解出大地坐标系下载具的横纵坐标以及航向角度x_car、y_car、θ_car；

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

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

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

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

v_X＝k_rDω_X(X＝1,2,3) (1)

其中v_X(X＝1,2,3)为动力轮的相对移速，ω_X(X＝1,2,3)为动力轮电机转速，k_r为电机减速比，D为动力轮的直径。

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

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

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

ω_err＝ω_exp-ω_out (2)

其中ω_exp为期望转速，ω_out为实际转速，ω_err为在前馈控制下控制转速的误差；

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

转矩误差T_err与转速误差ω_err近似线性相关，将转速误差ω_err经过低通滤波后得到的ω_{err_LPF}消除了噪声影响，此时通过近似线性公式计算出转矩误差T_err，具体公式如下：

T_err＝k_wω_{err_LPF} (3)

其中k_w为线性化系数，转矩误差T_err即为电机负载偏离正常情况的数值；

S33.根据步骤S32中得到的转矩误差T_err，对电机的运行状态做分类并进行修正：

当差值的绝对值小于等于正常波动阈值T_thres时，记电机处于状态1；

当差值的绝对值大于正常波动阈值阈值T_thres且差值为正时，记电机处于状态2；

当差值的绝对值大于正常波动阈值阈值T_thres且差值为负时，记电机处于状态3；

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

ω_{X_corr}＝ω_X，(X＝1,2,3) (4)

对于状态2，电机负载小于额定值，意味着电机与地面接触不良，可能出现了打滑，此时将电机转速设为ω_X和0经过式(1)得到轮子外廓相对移速后分别代入式(5)计算估计下次的坐标得到两组坐标(x_ω,y_ω)、(x₀,y₀)：

其中x_t、y_t为当前时刻的横、纵坐标，x_t+1、y_t+1为下一时刻的横、纵坐标，t_Δ为两次计算时间的间隔，将电机转速设为ω_X代入计算得到的结果x_t+1、y_t+1分别记为x_ω、y_ω，将电机转速设为0代入计算得到的结果x_t+1、y_t+1分别记为x₀、y₀，

与下时刻超宽带定位返回的位置(x_UWB，y_UWB)分别计算其欧氏距离d_ω、d₀，公式如下：

此时修正值为

对于状态3，电机负载大于额定值，意味着电机重载，此时可能行驶到阻力较大区域，也可能是碰到障碍而无法继续行进，此时将电机转速设为ω_X和0经过式(1)得到轮子外廓相对移速后分别带入式(5)计算估计下次的坐标得到(x_ω,y_ω)、(x₀,y₀)，与下时刻超宽带定位返回的位置分别计算其欧氏距离d_ω、d₀，此时修正值参照式(7)得到。

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

S41.使用步骤S3中获得的电机转速修正值ω_{X_corr}(X＝1,2,3)，根据式(1)计算动力轮外廓行进速度修正值v_{X_corr}(X＝1,2,3),得到三个动力轮外廓行进速度后，对其进行变换，得到机体直角坐标系下的速度分量：

其中[v₁ v₂ v₃]^T表示三个动力轮外廓行进速度修正值，r为载具动力轮到载具中心的距离，[v_x v_y ω_z]^T中三个元素分别为载具机体直角坐标系下的横、纵速度分量以及航向轴的转速；

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

其中θ为机体坐标系与大地坐标系的偏角，也即是载具的航向角，[v'_x v'_y ω'_z]^T中三个元素分别为大地直角坐标系下的横、纵速度分量以及航向轴的转速，其中M^T表示矩阵M的转置；

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

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

其中

为t-1时刻卡尔曼滤波后的位置；

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

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

为t时刻的预测值，

为t-1时刻的卡尔曼估计值；

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

为t时刻估计值的协方差矩阵，P_t-1为t-1时刻滤波输出值的协方差矩阵，Q为式(12.1)计算引入误差的协方差矩阵，A^T表示矩阵A的转置；

式(12.3)计算出卡尔曼增益K_t，H为UWB定位数据转换到编码器数据转换矩阵，R为UWB定位数据误差的协方差矩阵，H^T表示UWB定位数据转换到编码器数据转换矩阵H的转置；

式(12.4)根据卡尔曼增益计算位置的最优估计数值，Z_t为t时刻UWB定位数据，

为为t时刻的卡尔曼估计值；

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

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

其中

为t时刻卡尔曼滤波的位置估计结果。

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

Claims (5)

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

S1.前馈控制：使用前馈PID算法对每个电机的转速做闭环控制，基于编码器数据获得三个动力轮的转速ω₁、ω₂、ω₃，根据3个动力轮的尺寸得到相应的动力轮的相对移速；

S2.超宽带定位：基于UWB定位***获得全向轮载具的绝对位置坐标(x_UWB,y_UWB)；

S3.打滑修正：根据电机负载状态估计出每个动力轮的相对移速修正值ν_{X_corr}，(X＝1,2,3)，基于模型解算得到位置大地坐标系下载具的横纵移速以及航向转速v_{x_car}、v_{y_car}、ω_car基于状态方程解出大地坐标系下载具的横纵坐标以及航向角度x_car、y_car、θ_car；

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

2.根据权利要求1所述的涉及动力轮打滑修正的多传感器融合载具定位方法，其特征在于，步骤S1中所述前馈控制的具体方法是：

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

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

v_X＝k_rDω_X(X＝1,2,3) (1)

其中v_X(X＝1,2,3)为动力轮的相对移速，ω_X(X＝1,2,3)为动力轮电机转速，k_r为电机减速比，D为动力轮的直径。

3.根据权利要求1所述的涉及动力轮打滑修正的多传感器融合载具定位方法，其特征在于，步骤S2中所述超宽带定位的具体方法是：在全向轮载具上挂载的超宽带标签与周边的基站通讯并测距，通过最小二乘估计获得全向轮载具的绝对位置(x_UWB,y_UWB)。

4.根据权利要求1所述的涉及动力轮打滑修正的多传感器融合载具定位方法，其特征在于，步骤S3中所述打滑修正的具体方法是：

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

ω_err＝ω_exp-ω_out (2)

其中ω_exp为期望转速，ω_out为实际转速，ω_err为在前馈控制下控制转速的误差；

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

转矩误差T_err与转速误差ω_err近似线性相关，将转速误差ω_err经过低通滤波后得到的ω_{err_LPF}消除了噪声影响，此时通过近似线性公式计算出转矩误差T_err，具体公式如下：

T_err＝k_wω_{err_LPF} (3)

其中k_w为线性化系数，转矩误差T_err即为电机负载偏离正常情况的数值；

S33.根据步骤S32中得到的转矩误差T_err，对电机的运行状态做分类并进行修正：

当差值的绝对值小于等于正常波动阈值T_thres时，记电机处于状态1；

当差值的绝对值大于正常波动阈值阈值T_thres且差值为正时，记电机处于状态2；

当差值的绝对值大于正常波动阈值阈值T_thres且差值为负时，记电机处于状态3；

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

ω_{X_corr}＝ω_X，(X＝1,2,3) (4)

对于状态2，电机负载小于额定值，意味着电机与地面接触不良，可能出现了打滑，此时将电机转速设为ω_X和0经过式(1)得到轮子外廓相对移速后分别代入式(5)计算估计下次的坐标得到两组坐标(x_ω,y_ω)、(x₀,y₀)：

其中x_t、y_t为当前时刻的横、纵坐标，x_t+1、y_t+1为下一时刻的横、纵坐标，t_Δ为两次计算时间的间隔，将电机转速设为ω_X代入计算得到的结果x_t+1、y_t+1分别记为x_ω、y_ω，将电机转速设为0代入计算得到的结果x_t+1、y_t+1分别记为x₀、y₀，

与下时刻超宽带定位返回的位置(x_UWB，y_UWB)分别计算其欧氏距离d_ω、d₀，公式如下：

此时修正值为

对于状态3，电机负载大于额定值，意味着电机重载，此时可能行驶到阻力较大区域，也可能是碰到障碍而无法继续行进，此时将电机转速设为ω_X和0经过式(1)得到轮子外廓相对移速后分别带入式(5)计算估计下次的坐标得到(x_ω,y_ω)、(x₀,y₀)，与下时刻超宽带定位返回的位置分别计算其欧氏距离d_ω、d₀，此时修正值参照式(7)得到。

5.根据权利要求1所述的涉及动力轮打滑修正的多传感器融合载具定位方法，其特征在于，步骤S4中所述数据融合的具体方法是：

S41.使用步骤S3中获得的电机转速修正值ω_{X_corr}(X＝1,2,3)，根据式(1)计算动力轮外廓行进速度修正值v_{X_corr}(X＝1,2,3),得到三个动力轮外廓行进速度后，对其进行变换，得到机体直角坐标系下的速度分量：

其中[v₁ v₂ v₃]^T表示三个动力轮外廓行进速度修正值，r为载具动力轮到载具中心的距离，[v_x v_y ω_z]^T中三个元素分别为载具机体直角坐标系下的横、纵速度分量以及航向轴的转速；

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

其中θ为机体坐标系与大地坐标系的偏角，也即是载具的航向角，[v'_x v'_y ω'_z]^T中三个元素分别为大地直角坐标系下的横、纵速度分量以及航向轴的转速，其中M^T表示矩阵M的转置；

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

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

其中

为t-1时刻卡尔曼滤波后的位置；

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

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

为t时刻的预测值，

为t-1时刻的卡尔曼估计值；

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

为t时刻估计值的协方差矩阵，P_t-1为t-1时刻滤波输出值的协方差矩阵，Q为式(12.1)计算引入误差的协方差矩阵，A^T表示矩阵A的转置；

式(12.3)计算出卡尔曼增益K_t，H为UWB定位数据转换到编码器数据转换矩阵，R为UWB定位数据误差的协方差矩阵，H^T表示UWB定位数据转换到编码器数据转换矩阵H的转置；

式(12.4)根据卡尔曼增益计算位置的最优估计数值，Z_t为t时刻UWB定位数据，

为为t时刻的卡尔曼估计值；

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

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

其中

为t时刻卡尔曼滤波的位置估计结果。

Images