一种地下矿井道路无人车辆协同控制方法和系统与流程

1.本发明属于矿井无人车辆协同技术领域，特别涉及一种地下矿井道路无人车辆协同控制方法和系统。

背景技术：

2.由颗粒物(如泥沙或沙粒)遗撒造成的路面湿滑会造成车辆行驶附着系数降低，增加车辆的纵向制动距离和横向行驶稳定性，容易引发追尾、侧滑等危险工况，危害井下车辆行驶安全。
3.现有方案大部分是通过附加监测摄像头识别积水、结冰、遗撒等信息来实现，限制了对道路湿滑这一危险情况的准确、精细化预警和管理。对路面湿滑情况的管理主要是摄像头检测路面，或者通过附加激光发射模块等，利用视频检测技术将反射的激光波束进行视频算法分析。所以通过摄像头等感知方案对路面进行监测预警的方法则需要大量的摄像头等设施，需要极大的建设和维护成本，同时受光照、天气、物体遮挡等比较严重。

技术实现要素：

4.本发明提出了一种地下矿井道路无人车辆协同控制方法和系统，附着系数估计与车路协同系统深度融合，将车辆估计结果与路侧和云端共享，提高车辆协同管控能力。
5.为了实现上述目的，本发明提出了一种地下矿井道路无人车辆协同控制方法，包括以下步骤：
6.根据车辆行驶信息建立车辆各个方向加速度的方程；所述加速度方程包括前进方向的加速度、垂直前进方法的加速度和车辆横摆角加速度；
7.根据车辆各个方向加速度的方程，确定方程所代表的第一测量参数与第一输入参数、第一待估计参数之间的函数关系，计算第一测量参数的第一雅克比矩阵；
8.通过路测传感器获取路测第二测量变量，确定第二测量参数、第二待估计参数和第一输入参数的函数关系；计算第二测量参数的第二雅克比矩阵；
9.根据所述第一雅克比矩阵和第二雅克比矩阵，利用阻尼卡尔曼滤波，进行迭代运算，计算第一待估计参数值；将计算出的第一待估计参数值分享给其他车辆实现协同控制。
10.进一步的，根据车辆行驶信息建立车辆各个方向加速度的方程为：
11.根据车辆行驶动力学可得到：
[0012][0013][0014]
[0015]
其中，a
x
、ay分别为车辆沿纵向和横向的加速度；μf、μr分别为前轮和后轮的路面附着系数；为车辆前轮纵向提供的归一化力；为车辆前轮横向提供的归一化力；为车辆后轮纵向提供的归一化力；为车辆后轮横向提供的归一化力；x、y分别表示车辆纵向和横向，f、r分别代表前轮、后轮；m为车辆质量；σ为前轮转角，为车辆横摆角加速度，lf、lr分别表示车辆质心到前轴和后轴的距离，iz为车辆的横摆转动惯量。
[0016]
进一步的，所述根据车辆各个方向加速度的方程，确定方程所代表的第一测量参数与第一输入参数、第一待估计参数之间的函数关系；
[0017]
将a
x
、ay、作为第一测量参数记为y(t)；其中
[0018]
将车辆前轮转角σ作为第一输入参数记为u(t)；其中u(t)＝[δ]；
[0019]
将μf、μr、m作为第一待估计参数记为x
p
(t)；其中x
p
(t)＝[uf,ur,m]
t
；
[0020]
所以第一测量参数、第一输入参数、第一待估计参数抽象为函数h()；所述y(t)＝h(x
p
(t),u(t),w(t))；其中w(t)为第一测量误差；
[0021]
计算h(x
p
(t),u(t),w(t))对第一待估计参数的偏导，得到第一雅克比矩阵h(t)。
[0022]
进一步的，所述通过路测传感器获取路测第二测量变量，确定第二测量参数、第二待估计参数和第一输入参数的函数关系的过程为：
[0023]
通过路测传感器获取路测第二测量参数y
_r
(t)，包括a
x_r
、a
y_r
分别为车辆沿纵向路测加速度和车辆沿横向路测加速度；所以y
_r
(t)＝h(x
p_r
(t),u(t),wd(t))；x
p_r
(t)为第二估计参数；
[0024]
计算h(x
p_r
(t),u(t),wd(t))对第二估计参数的偏导，得到第二测量对应的第二雅克比矩阵hd(t)；其中wd(t)第二测量误差。
[0025]
进一步的，所述根据所述第一雅克比矩阵和第二雅克比矩阵，利用阻尼卡尔曼滤波，进行迭代运算，计算第一待估计参数值的过程为：
[0026]
给定迭代计算的初值；在迭代计算中引入阻尼项，用于对强瞬态变量的误差抑制；计算卡尔曼滤波中的卡尔曼增益以及对本时间步的第一待估计参数进行校正；校正完成后，将时间步的时间更新为上一时间步，根据校正值再对当前时间步的第一待估计参数进行预测。
[0027]
进一步的，所述给定迭代计算的初值包括：随机给定第一估计参数向量x
p
(t)的初始估计值并指定随机的初始协方差矩阵和初始阻尼协方差矩阵作为迭代计算的初值。
[0028]
进一步的，所述在迭代计算中引入阻尼项，用于对强瞬态变量的误差抑制的过程为：
[0029]
计算t时刻阻尼增益其中，为预测到的t时刻阻尼协方差矩阵估计值；rd为第二测量参数高斯白噪声协方差矩阵；
[0030]
根据阻尼增益，利用第一测量参数y(t)和第二测量参数y
_r
(t)，计算第一测量参数y(t)和第二测量参数y
_r
(t)之间的第三测量参数y
_d
(t)；其中：
[0031]
通过阻尼增益的作用，实现对的抑制，从而实现对第二测量参数强瞬态变量的误差抑制。
[0032]
进一步的，所述计算卡尔曼滤波中的卡尔曼增益以及对本时间步的第一待估计参数进行校正的过程包括：
[0033]
计算卡尔曼滤波中的卡尔曼增益k(t)的公式为：其中，为预测到的t时刻协方差矩阵估计值；r为第一测量参数高斯白噪声协方差矩阵；
[0034]
利用校正方程计算t时刻第一测量参数x
p
(t)；其中为t时刻第一测量参数的估计量；代表y(t)＝h(x
p
(t),u(t),w(t))在t时刻的第一输入参数u(t)、测量误差为0时的计算结果；
[0035]
利用状态误差协方差校正方程利用状态误差协方差校正方程
[0036]
更新t时刻协方差矩阵校正值p(t)和t时刻阻尼协方差矩阵校正值p
_d
(t)。
[0037]
进一步的，所述将时间步的时间更新为上一时间步，根据校正值再对当前时间步的第一待估计参数进行预测的过程包括：
[0038]
在t时刻校正弯沉改后，进行时间步的更新，将计算的t时刻变为t-1时刻，下一时刻更新为t时刻；
[0039]
根据x
p
(t-1)、u(t)，通过x
p
(t)＝f(x
p
(t-1),u(t),0)，计算t时刻第一测量参数的估计量
[0040]
根据t-1时刻协方差矩阵校正值p(t-1)、t时刻阻尼协方差矩阵校正值以及以及激励噪声的协方差q计算t时刻协方差矩阵估计值和阻尼协方差矩阵估计值即：
[0041]
然后再进行t时刻的迭代运算；最后分别得到μf、μr、m的值。
[0042]
本发明还提出了一种地下矿井道路无人车辆协同控制系统，包括建立模块、第一确定模块、第二确定模块和计算模块；
[0043]
所述建立模块用于根据车辆行驶信息建立车辆各个方向加速度的方程；所述加速度方程包括前进方向的加速度、垂直前进方法的加速度和车辆横摆角加速度；
[0044]
所述第一确定模块用于根据车辆各个方向加速度的方程，确定方程所代表的第一测量参数与第一输入参数、第一待估计参数之间的函数关系，计算第一测量参数的第一雅克比矩阵；
[0045]
所述第二确定模块用于通过路测传感器获取路测第二测量变量，确定第二测量参数、第二待估计参数和第一输入参数的函数关系；计算第二测量参数的第二雅克比矩阵；
[0046]
所述计算模块用于根据所述第一雅克比矩阵和第二雅克比矩阵，利用阻尼卡尔曼滤波，进行迭代运算，计算第一待估计参数值；将计算出的第一待估计参数值分享给其他车辆实现协同控制。
[0047]
发明内容中提供的效果仅仅是实施例的效果，而不是发明所有的全部效果，上述
技术方案中的一个技术方案具有如下优点或有益效果：
[0048]
本发明提出了一种地下矿井道路无人车辆协同控制方法和系统，该方法包括：根据车辆行驶信息建立车辆各个方向加速度的方程；加速度方程包括前进方向的加速度、垂直前进方法的加速度和车辆横摆角加速度；根据车辆各个方向加速度的方程，确定方程所代表的第一测量参数与第一输入参数、第一待估计参数之间的函数关系，计算第一测量参数的第一雅克比矩阵；通过路测传感器获取路测第二测量变量，确定第二测量参数、第二待估计参数和第一输入参数的函数关系；计算第二测量参数的第二雅克比矩阵；根据第一雅克比矩阵和第二雅克比矩阵，利用阻尼卡尔曼滤波，进行迭代运算，计算第一待估计参数值；将计算出的第一待估计参数值分享给其他车辆实现协同控制。基于一种地下矿井道路无人车辆协同控制方法，还提出了一种地下矿井道路无人车辆协同控制系统。本发明在估计路面附着系数算法中，引入了智能路侧对车辆加速度等信息的测量，避免了传统方法单一通过车身传感器进行测量带来的误差；针对轮胎打滑的强瞬态过程，通过阻尼项的引入，实现对强瞬态变量的误差抑制，建立了阻尼卡尔曼滤波方法，提升了滤波算法对该工况的适应能力；附着系数估计与车路协同系统深度融合，将车辆估计结果与路侧和云端共享，提高车辆协同管控能力。
附图说明
[0049]
如图1为本发明实施例1一种地下矿井道路无人车辆协同控制方法流程图；
[0050]
如图2为本发明实施例1中阻尼卡尔曼迭代计算流程示意图；
[0051]
如图3为本发明实施例2一种地下矿井道路无人车辆协同控制系统示意图。
具体实施方式
[0052]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0053]
实施例1
[0054]
本发明实施例1提出了一种地下矿井道路无人车辆协同控制方法，基于阻尼卡尔曼滤波(dkf)方法的路面附着系数在线估计和基于车路协同的道路湿滑预警。其中，阻尼卡尔曼滤波(dkf)为参考扩展卡尔曼滤波(ekf)改进更新的一种算法，主要解决在有转速进行车速估计时，由变量的强瞬态变化造成的误差，主要通过在增益矩阵中增加阻尼项来实现对强瞬态变量的抑制。
[0055]
如图1为本发明实施例1一种地下矿井道路无人车辆协同控制方法流程图；
[0056]
在步骤s100中，根据车辆行驶信息建立车辆各个方向加速度的方程；加速度方程包括前进方向的加速度、垂直前进方法的加速度和车辆横摆角加速度；
[0057]
根据车辆行驶动力学可得到：
[0058]
[0059][0060][0061]
其中，a
x
、ay分别为车辆沿纵向和横向的加速度；μf、μr分别为前轮和后轮的路面附着系数；为车辆前轮纵向提供的归一化力；为车辆前轮横向提供的归一化力；为车辆后轮纵向提供的归一化力；为车辆后轮横向提供的归一化力；x、y分别表示车辆纵向和横向，f、r分别代表前轮、后轮；m为车辆质量；σ为前轮转角，为车辆横摆角加速度，lf、lr分别表示车辆质心到前轴和后轴的距离，iz为车辆的横摆转动惯量。
[0062]
在步骤s110中，根据车辆各个方向加速度的方程，确定方程所代表的第一测量参数与第一输入参数、第一待估计参数之间的函数关系，计算第一测量参数的第一雅克比矩阵；
[0063]
在车辆行驶动力学方程中，因为a
x
、ay、由车载惯导元件提供，因此将a
x
、ay、作为第一测量参数记为y(t)；其中
[0064]
将车辆前轮转角σ作为第一输入参数记为u(t)；其中u(t)＝[δ]；
[0065]
为了排除可能的车辆载重变化对估计结果的影响，将车辆质量m也作为待估计参数变量，所以将μf、μr、m作为第一待估计参数记为x
p
(t)；其中x
p
(t)＝[uf,ur,m]
t
；
[0066]
所以第一测量参数、第一输入参数、第一待估计参数抽象为函数h()；y(t)＝h(x
p
(t),u(t),w(t))；其中w(t)为第一测量误差；即h(x
p
(t),u(t),w(t))代表了第一输入参数、第一待估计参数和第一测量误差之间的函数关系。计算h(x
p
(t),u(t),w(t))对第一待估计参数的偏导，得到第一雅克比矩阵h(t)。
[0067]
在步骤s120中，通过路测传感器获取路测第二测量变量，确定第二测量参数、第二待估计参数和第一输入参数的函数关系；计算第二测量参数的第二雅克比矩阵；
[0068]
通过路测传感器获取车辆加速度，包括a
x_r
、a
y_r
分别为车辆沿纵向路测加速度和车辆沿横向路测加速度；记为：路测第二测量参数y
_r
(t)，所以y
_r
(t)＝h(x
p_r
(t),u(t),wd(t))；x
p_r
(t)为第二估计参数；
[0069]
根据第二测量参数y
_r
(t)与第二估计参数x
p_r
(t)、第一输入参数u(t)和第二测量误差wd(t)之间的函数关系，计算h(x
p_r
(t),u(t),wd(t))对第二估计参数的偏导，得到第二测量对应的第二雅克比矩阵hd(t)；其中wd(t)第二测量误差。
[0070]
在步骤s130中，根据所述第一雅克比矩阵和第二雅克比矩阵，利用阻尼卡尔曼滤波，进行迭代运算，计算第一待估计参数值；将计算出的第一待估计参数值分享给其他车辆实现协同控制。如图2为本发明实施例1中阻尼卡尔曼迭代计算流程示意图；具体的过程为：
[0071]
给定迭代计算的初值；在迭代计算中引入阻尼项，用于对强瞬态变量的误差抑制；计算卡尔曼滤波中的卡尔曼增益以及对本时间步的第一待估计参数进行校正；校正完成后，将时间步的时间更新为上一时间步，根据校正值再对当前时间步的第一待估计参数进行预测。
[0072]
给定迭代计算的初值，即图2中的输入模块：随机给定第一估计参数向量x
p
(t)的初始估计值并指定随机的初始协方差矩阵和初始阻尼协方差矩阵作为迭代计算的初始值，随后开始迭代计算。
[0073]
在迭代计算中引入阻尼项，用于对强瞬态变量的误差抑制，即图2中的阻尼模块，执行的过程为：
[0074]
计算t时刻阻尼增益其中，为预测到的t时刻阻尼协方差矩阵估计值；rd为第二测量参数高斯白噪声协方差矩阵；
[0075]
根据阻尼增益，利用第一测量参数y(t)和第二测量参数y
_r
(t)，计算第一测量参数y(t)和第二测量参数y
_r
(t)之间的第三测量参数y
_d
(t)；其中：
[0076]
通过阻尼增益的作用，实现对的抑制，从而实现对第二测量参数强瞬态变量的误差抑制。
[0077]
计算卡尔曼滤波中的卡尔曼增益以及对本时间步的第一待估计参数进行校正，即图2中的校正模块，执行的过程为：
[0078]
计算卡尔曼滤波中的卡尔曼增益k(t)的公式为：其中，为预测到的t时刻协方差矩阵估计值；r为第一测量参数高斯白噪声协方差矩阵；
[0079]
利用校正方程计算t时刻第一测量参数x
p
(t)；其中为t时刻第一测量参数的估计量；代表y(t)＝h(x
p
(t),u(t),w(t))在t时刻的第一输入参数u(t)、测量误差为0时的计算结果；
[0080]
利用状态误差协方差校正方程利用状态误差协方差校正方程
[0081]
更新t时刻协方差矩阵校正值p(t)和t时刻阻尼协方差矩阵校正值p
_d
(t)。
[0082]
将时间步的时间更新为上一时间步，根据校正值再对当前时间步的第一待估计参数进行预测，即图2中的预测模块。
[0083]
在t时刻校正完成后，进行时间步的更新，将计算的t时刻变为t-1时刻，下一时刻更新为t时刻；
[0084]
根据x
p
(t-1)、u(t)，通过x
p
(t)＝f(x
p
(t-1),u(t),0)，计算t时刻第一测量参数的估计量
[0085]
根据t-1时刻协方差矩阵校正值p(t-1)、t时刻阻尼协方差矩阵校正值以及以及激励噪声的协方差q计算t时刻协方差矩阵估计值和阻尼协方差矩阵估计值即：其中协方差q可根据实际情况给定或调整。
[0086]
得到协方差矩阵估计值和阻尼协方差矩阵估计值之后分别反馈至阻尼模块和校正模块，再次进行更新后的t时刻迭代计算。
[0087]
通过上面的计算流程，可以得到利用第一测量参数、第二测量变量、协方差矩阵估计值和阻尼协方差矩阵估计值对第一待估计参数x
p
(t)，即得到μf、μr、m的值。
[0088]
在得到μf、μr、m的值后，附着系数μf、μr的数值则代表了车辆此时行驶路面的附着状况，当附着系数约为0.85左右时，说明处于干燥的良好路面，0.2-0.4时可能处于较为湿滑的路面，小于0.2时则代表结冰、沙粒等高危情况。本发明保护的数据不局限于实施例1中列出的具体数据，本领域人员可以根据实际情况进行设定或者判定。
[0089]
根据具体数值，可以通过车载obu发送给其他车辆的obu终端或者路侧rsu终端，由rsu向车端进行广播(所涉及到的通讯包括但不限于4g-lte、5g、dsrc等通讯手段)，其他车辆接收到相关信息后，可以通过声音、文字等形式为驾驶员提供预警信息，使驾驶员提前减速、注意车辆操纵等保证驾驶安全，同时可由rsu传给相应的后端管控平台，为井下交通管控等提供参考，实现基于车路协同的车辆协同控制。
[0090]
本发明实施例1提出的一种地下矿井道路无人车辆协同控制方法，在估计路面附着系数算法中，引入了智能路侧对车辆加速度等信息的测量，避免了传统方法单一通过车身传感器进行测量带来的误差；
[0091]
本发明实施例1针对轮胎打滑的强瞬态过程，通过阻尼项的引入，实现对强瞬态变量的误差抑制，建立了阻尼卡尔曼滤波方法，提升了滤波算法对该工况的适应能力；
[0092]
本发明实施例1附着系数估计与车路协同系统深度融合，将车辆估计结果与路侧和云端共享，提高车辆协同管控能力。
[0093]
实施例2
[0094]
基于本发明实施例1提出的一种地下矿井道路无人车辆协同控制方法，本发明实施例2还提出了一种地下矿井道路无人车辆协同控制系统，如图3为本发明实施例2提出的一种地下矿井道路无人车辆协同控制系统示意图，该系统包括：建立模块、第一确定模块、第二确定模块和计算模块；
[0095]
建立模块用于根据车辆行驶信息建立车辆各个方向加速度的方程；所述加速度方程包括前进方向的加速度、垂直前进方法的加速度和车辆横摆角加速度；
[0096]
第一确定模块用于根据车辆各个方向加速度的方程，确定方程所代表的第一测量参数与第一输入参数、第一待估计参数之间的函数关系，计算第一测量参数的第一雅克比矩阵；
[0097]
第二确定模块用于通过路测传感器获取路测第二测量变量，确定第二测量参数、第二待估计参数和第一输入参数的函数关系；计算第二测量参数的第二雅克比矩阵；
[0098]
计算模块用于根据所述第一雅克比矩阵和第二雅克比矩阵，利用阻尼卡尔曼滤波，进行迭代运算，计算第一待估计参数值；将计算出的第一待估计参数值分享给其他车辆实现协同控制。
[0099]
本发明实施例2提出的一种地下矿井道路无人车辆协同控制系统，将实施例1方法实现的过程模块化，具体实现的过程参照实施例1方法实现的过程。
[0100]
本发明实施例2提出的一种地下矿井道路无人车辆协同控制系统，在估计路面附
着系数算法中，引入了智能路侧对车辆加速度等信息的测量，避免了传统方法单一通过车身传感器进行测量带来的误差；
[0101]
本发明实施例2针对轮胎打滑的强瞬态过程，通过阻尼项的引入，实现对强瞬态变量的误差抑制，建立了阻尼卡尔曼滤波方法，提升了滤波算法对该工况的适应能力；
[0102]
本发明实施例2附着系数估计与车路协同系统深度融合，将车辆估计结果与路侧和云端共享，提高车辆协同管控能力。
[0103]
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个
……”
限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。另外，本技术实施例提供的上述技术方案中与现有技术中对应技术方案实现原理一致的部分并未详细说明，以免过多赘述。
[0104]
上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制。对于所属领域的技术人员来说，在上述说明的基础上还可以做出其它不同形式的修改或变形。这里无需也无法对所有的实施方式予以穷举。在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

技术特征：

1.一种地下矿井道路无人车辆协同控制方法，其特征在于，包括以下步骤：根据车辆行驶信息建立车辆各个方向加速度的方程；所述加速度方程包括前进方向的加速度、垂直前进方法的加速度和车辆横摆角加速度；根据车辆各个方向加速度的方程，确定方程所代表的第一测量参数与第一输入参数、第一待估计参数之间的函数关系，计算第一测量参数的第一雅克比矩阵；通过路测传感器获取路测第二测量变量，确定第二测量参数、第二待估计参数和第一输入参数的函数关系；计算第二测量参数的第二雅克比矩阵；根据所述第一雅克比矩阵和第二雅克比矩阵，利用阻尼卡尔曼滤波，进行迭代运算，计算第一待估计参数值；将计算出的第一待估计参数值分享给其他车辆实现协同控制。2.根据权利要求1所述的一种地下矿井道路无人车辆协同控制方法，其特征在于，根据车辆行驶信息建立车辆各个方向加速度的方程为：根据车辆行驶动力学可得到：根据车辆行驶动力学可得到：根据车辆行驶动力学可得到：其中，a
x
、a
y
分别为车辆沿纵向和横向的加速度；μ
f
、μ
f
分别为前轮和后轮的路面附着系数；为车辆前轮纵向提供的归一化力；为车辆前轮横向提供的归一化力；为车辆后轮纵向提供的归一化力；为车辆后轮横向提供的归一化力；x、y分别表示车辆纵向和横向，f、r分别代表前轮、后轮；m为车辆质量；σ为前轮转角，为车辆横摆角加速度，l
f
、l
r
分别表示车辆质心到前轴和后轴的距离，i
z
为车辆的横摆转动惯量。3.根据权利要求2所述的一种地下矿井道路无人车辆协同控制方法，其特征在于，所述根据车辆各个方向加速度的方程，确定方程所代表的第一测量参数与第一输入参数、第一待估计参数之间的函数关系；将a
x
、a
y
、作为第一测量参数记为y(t)；其中将车辆前轮转角σ作为第一输入参数记为u(t)；其中u(t)＝[δ]；将μ
f
、μ
r
、m作为第一待估计参数记为x
p
(t)；其中x
p
(t)＝[u
f
,u
r
,m]
t
；所以第一测量参数、第一输入参数、第一待估计参数抽象为函数h()；所述y(t)＝h(x
p
(t),u(t),w(t))；其中w(t)为第一测量误差；计算h(x
p
(t),u(t),w(t))对第一待估计参数的偏导，得到第一雅克比矩阵h(t)。4.根据权利要求3所述的一种地下矿井道路无人车辆协同控制方法，其特征在于，所述通过路测传感器获取路测第二测量变量，确定第二测量参数、第二待估计参数和第一输入参数的函数关系的过程为：通过路测传感器获取路测第二测量参数y
_r
(t)，包括a
x_r
、a
y_r
分别为车辆沿纵向路测加
速度和车辆沿横向路测加速度；所以y
_r
(t)＝h(x
p_r
(t),u(t),w
d
(t))；x
p_r
(t)为第二估计参数；计算h(x
p_r
(t),u(t),w
d
(t))对第二估计参数的偏导，得到第二测量对应的第二雅克比矩阵h
d
(t)；其中w
d
(t)第二测量误差。5.根据权利要求4所述的一种地下矿井道路无人车辆协同控制方法，其特征在于，所述根据所述第一雅克比矩阵和第二雅克比矩阵，利用阻尼卡尔曼滤波，进行迭代运算，计算第一待估计参数值的过程为：给定迭代计算的初值；在迭代计算中引入阻尼项，用于对强瞬态变量的误差抑制；计算卡尔曼滤波中的卡尔曼增益以及对本时间步的第一待估计参数进行校正；校正完成后，将时间步的时间更新为上一时间步，根据校正值再对当前时间步的第一待估计参数进行预测。6.根据权利要求5所述的一种地下矿井道路无人车辆协同控制方法，其特征在于，所述给定迭代计算的初值包括：随机给定第一估计参数向量x
p
(t)的初始估计值并指定随机的初始协方差矩阵和初始阻尼协方差矩阵作为迭代计算的初值。7.根据权利要求6所述的一种地下矿井道路无人车辆协同控制方法，其特征在于，所述在迭代计算中引入阻尼项，用于对强瞬态变量的误差抑制的过程为：计算t时刻阻尼增益其中，为预测到的t时刻阻尼协方差矩阵估计值；r
d
为第二测量参数高斯白噪声协方差矩阵；根据阻尼增益，利用第一测量参数y(t)和第二测量参数y
_r
(t)，计算第一测量参数y(t)和第二测量参数y
_r
(t)之间的第三测量参数y
_d
(t)；其中：通过阻尼增益的作用，实现对的抑制，从而实现对第二测量参数强瞬态变量的误差抑制。8.根据权利要求7所述的一种地下矿井道路无人车辆协同控制方法，其特征在于，所述计算卡尔曼滤波中的卡尔曼增益以及对本时间步的第一待估计参数进行校正的过程包括：计算卡尔曼滤波中的卡尔曼增益k(t)的公式为：其中，为预测到的t时刻协方差矩阵估计值；r为第一测量参数高斯白噪声协方差矩阵；利用校正方程计算t时刻第一测量参数x
p
(t)；其中为t时刻第一测量参数的估计量；代表y(t)＝h(x
p
(t),u(t),w(t))在t时刻的第一输入参数u(t)、测量误差为0时的计算结果；利用状态误差协方差校正方程更新t时刻协方差矩阵校正值p(t)和t时刻阻尼协方差矩阵校正值p
_d
(t)。9.根据权利要求8所述的一种地下矿井道路无人车辆协同控制方法，其特征在于，所述将时间步的时间更新为上一时间步，根据校正值再对当前时间步的第一待估计参数进行预
测的过程包括：在t时刻校正完成后，进行时间步的更新，将计算的t时刻变为t-1时刻，下一时刻更新为t时刻；根据x
p
(t-1)、u(t)，通过x
p
(t)＝f(x
p
(t-1),u(t),0)，计算t时刻第一测量参数的估计量根据t-1时刻协方差矩阵校正值p(t-1)、t时刻阻尼协方差矩阵校正值以及以及激励噪声的协方差q计算t时刻协方差矩阵估计值和阻尼协方差矩阵估计值即：然后再进行t时刻的迭代运算；最后分别得到μ
f
、μ
r
、m的值。10.一种地下矿井道路无人车辆协同控制系统，其特征在于，包括：建立模块、第一确定模块、第二确定模块和计算模块；所述建立模块用于根据车辆行驶信息建立车辆各个方向加速度的方程；所述加速度方程包括前进方向的加速度、垂直前进方法的加速度和车辆横摆角加速度；所述第一确定模块用于根据车辆各个方向加速度的方程，确定方程所代表的第一测量参数与第一输入参数、第一待估计参数之间的函数关系，计算第一测量参数的第一雅克比矩阵；所述第二确定模块用于通过路测传感器获取路测第二测量变量，确定第二测量参数、第二待估计参数和第一输入参数的函数关系；计算第二测量参数的第二雅克比矩阵；所述计算模块用于根据所述第一雅克比矩阵和第二雅克比矩阵，利用阻尼卡尔曼滤波，进行迭代运算，计算第一待估计参数值；将计算出的第一待估计参数值分享给其他车辆实现协同控制。

技术总结

本发明提出了一种地下矿井道路无人车辆协同控制方法和系统，该方法包括：根据车辆行驶信息建立车辆各个方向加速度的方程；根据方程，确定方程所代表的第一测量参数与第一输入参数、第一待估计参数之间的函数关系，计算第一雅克比矩阵；通过路测传感器获取路测第二测量变量，确定第二测量参数、第二待估计参数和第一输入参数的函数关系；计算第二雅克比矩阵；根据第一雅克比矩阵和第二雅克比矩阵，利用阻尼卡尔曼滤波，进行迭代运算，计算第一待估计参数值；并分享给其他车辆实现协同控制。基于该方法，还提出了控制系统。本发明计算路面附着系数针对轮胎打滑的强瞬态过程，通过阻尼项的引入，实现对强瞬态变量的误差抑制，提高车辆协同能力。高车辆协同能力。高车辆协同能力。