CN111538331A - 一种地下无人铰接车的反应式导航方法 - Google Patents

Abstract

本发明提供一种地下无人铰接车的反应式导航方法，能够提高路径跟踪的精度。所述方法包括：感知铰接车周围的巷道环境；根据感知到的铰接车周围的巷道环境及铰接车在地下巷道的行驶路径，完成反应式导航过程中的分段式参考路径规划，其中，得到的分段式参考路径为基于铰接车车身坐标系的分段直线；根据规划完成的分段式参考路径，以优化铰接车对分段式参考路径的跟踪效果以及控制量输出的平顺性为目标，确定非线性模型预测控制的优化函数，得到最优控制序列，以便铰接车对规划完成的分段式参考路径进行跟踪。本发明涉及自主导航领域。

Description

一种地下无人铰接车的反应式导航方法

技术领域

本发明涉及自主导航领域，特别是指一种地下无人铰接车的反应式导航方法。

背景技术

近年来，随着矿产资源的不断开采，采矿环境也变得更加恶劣，利用矿车自主导航来代替人工驾驶完成地下采矿与运输工作，对于地下采矿工作的安全性和效率性都会有明显的提高。地下铰接车如铲运机由于具有良好的驱动能力与通过能力，成为了地下采矿运输工作中的重要设备。地下铰接车要实现自主行驶，就必须依靠先进的自主导航技术。

反应式导航，主要通过感知周围环境做出合理的预判，从而做出相应的决策，并不依赖传统的全局地图信息。

当前的反应式导航研究中仍然存在很多亟待解决的问题，如在地下巷道环境识别技术中，对于巷道环境的识别效果不佳，在路径跟踪过程中，路径跟踪效果较差，尤其体现在弯道处局部路径的跟踪效果上。以上问题严重制约了地下矿用车辆的自主导航的发展，因此需要在当前研究的基础上，对反应式导航的研究中存在的以上具体问题进行改进与解决，提高地下无人铰接车的自主行驶性能，使铰接车辆能够在地下采矿环境中识别巷道环境，并根据工作任务完成自主行驶过程。

发明内容

本发明要解决的技术问题是提供一种地下无人铰接车的反应式导航方法，以解决现有技术所存在的路径跟踪效果较差的问题。

为解决上述技术问题，本发明实施例提供一种地下无人铰接车的反应式导航方法，包括：

感知铰接车周围的巷道环境；

根据感知到的铰接车周围的巷道环境及铰接车在地下巷道的行驶路径，完成反应式导航过程中的分段式参考路径规划，其中，得到的分段式参考路径为基于铰接车车身坐标系的分段直线；

根据规划完成的分段式参考路径，以优化铰接车对分段式参考路径的跟踪效果以及控制量输出的平顺性为目标，确定非线性模型预测控制的优化函数，得到最优控制序列，以便铰接车对规划完成的分段式参考路径进行跟踪。

进一步地，分段式包括：直巷道行走阶段、进入弯道前阶段、进入弯道后阶段和转弯完成阶段。

进一步地，所述根据感知到的铰接车周围的巷道环境及铰接车在地下巷道的行驶路径，完成反应式导航过程中的分段式参考路径规划包括：

在直巷道行走阶段，根据转弯前的左侧巷道壁拟合直线l₁、右侧巷道壁拟合直线l₂相对于铰接车车身坐标系的直线方程求解巷道中心线a₁的方程，其中，得到的巷道中心线a₁基于铰接车车身坐标系方程为：

其中，巷道中心线a₁为直巷道行走阶段的参考路径，k₁为左侧巷道壁拟合直线l₁的斜率，c₁、c₂分别为直线l₁、l₂在铰接车车身坐标系y轴上的截距，x、y为巷道中心线a₁在铰接车车身坐标系x轴、y轴上的坐标。

进一步地，所述根据感知到的铰接车周围的巷道环境及铰接车在地下巷道的行驶路径，完成反应式导航过程中的分段式参考路径规划包括：

在进入弯道前阶段，将参考路径a₂设置为相应侧巷道与铰接车车身中心的安全距离为预设值的直线；其中，

在铰接车左转时，将参考路径a₂设置为距离铰接车右侧巷道壁预设值的平行直线，其中，参考路径a₂的方程为：

其中，θ₁为k₁的反正切值，t为预设值，k₁为转弯前的左侧巷道壁拟合直线l₁的斜率，c₂为右侧巷道壁拟合直线l₂在铰接车车身坐标系y轴上的截距，x、y为参考路径a₂在铰接车车身坐标系x轴、y轴上的坐标；

在铰接车右转时，将参考路径a₂设置为距离铰接车左侧巷道壁预设值的平行直线，其中，参考路径a₂的方程为：

其中，c₁为左侧巷道壁拟合直线l₁在铰接车车身坐标系y轴上的截距。

进一步地，所述根据感知到的铰接车周围的巷道环境及铰接车在地下巷道的行驶路径，完成反应式导航过程中的分段式参考路径规划包括：

在进入弯道后阶段，将转弯后的参考路径a₃设置为距离相应侧巷道壁预设值的平行直线；

其中，在铰接车左转时，将转弯后参考路径a₃设置为距离铰接车左侧巷道壁预设值的平行直线，其中，参考路径a₃为：

其中，θ₃为k₃的反正切值，k₁为转弯前的左侧巷道壁拟合直线l₁的斜率，k₃为转弯后的左巷道拟合直线l₃的斜率，t为预设值，c₃、c₄分别为转弯后的左巷道拟合直线l₃、转弯后的右巷道拟合直线l₄在铰接车车身坐标系y轴上的截距，x、y为参考路径a₃在铰接车车身坐标系x轴、y轴上的坐标；

其中，t为预设值，x为参考路径a₃在铰接车车身坐标系x轴上的坐标，c₅表示转弯后的右巷道拟合直线l₄在x轴的坐标值；

在铰接车右转时，将转弯后的参考路径a₃设置为距离铰接车右侧巷道壁预设值的平行直线，其中，参考路径a₃为：

进一步地，所述根据感知到的铰接车周围的巷道环境及铰接车在地下巷道的行驶路径，完成反应式导航过程中的分段式参考路径规划包括：

在转弯完成阶段，再次进入直巷道行走阶段，参考路径为巷道中心线。

进一步地，所述根据规划完成的分段式参考路径，以优化铰接车对分段式参考路径的跟踪效果以及控制量输出的平顺性为目标，确定非线性模型预测控制的优化函数，得到最优控制序列，以便铰接车对规划完成的分段式参考路径进行跟踪包括：

建立铰接车的运动学模型；

对建立的铰接车的运动学模型进行离散化，建立非线性的预测模型，预测模型根据当前时刻的状态量和控制量预测未来时刻的铰接车的位姿信息；

将分段式参考路径根据预测步长离散为连续的点，得到由点序列组成的参考路径；

基于得到的预测时域内铰接车的位姿信息和得到的由点序列组成的参考路径，以优化铰接车对分段式参考路径的跟踪效果以及控制量输出的平顺性为目标，确定非线性模型预测控制的优化函数，得到最优控制序列，以便铰接车对规划完成的分段式参考路径进行跟踪。

进一步地，建立的铰接车的运动学模型表示为：

其中，v_f、v_r分别为前车体、后车体的速度；γ为铰接角；w_f、w_r分别为前车体、后车体的横摆角速度；L_f、L_r分别为前车体、后车体的长度；

w

γ都表示铰接角速度。

进一步地，确定的非线性模型预测控制的优化函数J表示为：

其中，e(t)是指t时刻的跟踪误差，Δu(t)、u(t)分别指t时刻的控制增量和控制量，Q和R都表示权重矩阵，‖·‖表示范数，ρ为权重系数，ε为松弛因子；Np为预测时域；Nc为控制时域；e(t+i|t)＝x(t+i|t)-x_ref(t+i|t)，e(t+i|t)表示在t时刻第i个预测点的跟踪误差，x(t+i|t)为t时刻第i个预测点的状态量，状态量x＝[x_fy_fθ_fγ]，x_f、y_f分别表示前车体在X轴、Y轴方向上的位移，θ_f为前车体的航向角，x_ref(t+i|t)表示t时刻第i个预测点参考路径；Δu(t+i|t)＝u(t+i|t)-u(t|t)，Δu(t+i|t)为t时刻第i个预测点的控制增量，u(t+i|t)为t时刻第i个预测点的控制量，控制量u＝[v_fw_γ]；

确定的非线性模型预测控制的优化函数J的约束条件包括：

s.t.

v_fmin≤v_f≤v_fmax

w_γmin≤w_γ≤w_γmax

γ_min≤γ≤γ_max

其中，s.t.表示约束条件；v_fmin、v_fmax分别表示v_f的最小值、最大值；w_γmin、w_γmax分别表示w_γ的最小值、最大值；γ_min、γ_max分别表示γ的最小值、最大值。

进一步地，最优控制序列

其中，

为t时刻第1个最优控制量，将该最优控制序列中第一个元素

作为铰接车的实际控制量控制所述铰接车对规划完成的分段式参考路径进行跟踪。

本发明的上述技术方案的有益效果如下：

上述方案中，感知铰接车周围的巷道环境，为参考路径规划奠定基础；根据感知到的铰接车周围的巷道环境及铰接车在地下巷道的行驶路径，完成反应式导航过程中的分段式参考路径规划，其中，得到的分段式参考路径为基于铰接车车身坐标系的分段直线；根据规划完成的分段式参考路径，以优化铰接车对分段式参考路径的跟踪效果以及控制量输出的平顺性为目标，确定非线性模型预测控制的优化函数，得到最优控制序列，以便铰接车对规划完成的分段式参考路径进行跟踪，能够有效提高路径跟踪的精度。

附图说明

图1为本发明实施例提供的地下无人铰接车的反应式导航方法的流程示意图；

图2为本发明实施例提供的直巷道反应式导航控制策略示意图；

图3为本发明实施例提供的左转时反应式导航控制策略示意图；

图4为本发明实施例提供的右转时反应式导航控制策略示意图；

图5为本发明实施例提供的车身坐标系上的巷道壁拟合直线示意图；

图6为本发明实施例提供的左右巷道拟合直线在车身坐标系y轴的截距大小关系示意图；

图7为本发明实施例提供的地下无人铰接车的结构示意图。

具体实施方式

为使本发明要解决的技术问题、技术方案和优点更加清楚，下面将结合附图及具体实施例进行详细描述。

本发明针对现有的路径跟踪效果较差的问题，提供一种地下无人铰接车的反应式导航方法。

如图1所示，本发明实施例提供的地下无人铰接车的反应式导航方法，包括：

S101，感知铰接车周围的巷道环境；

S102，根据感知到的铰接车周围的巷道环境及铰接车在地下巷道的行驶路径，完成反应式导航过程中的分段式参考路径规划，其中，得到的分段式参考路径为基于铰接车车身坐标系的分段直线；

S103，根据规划完成的分段式参考路径，以优化铰接车对分段式参考路径的跟踪效果以及控制量输出的平顺性为目标，确定非线性模型预测控制的优化函数，得到最优控制序列，以便铰接车对规划完成的分段式参考路径进行跟踪。

本发明实施例所述的地下无人铰接车的反应式导航方法，感知铰接车周围的巷道环境，为参考路径规划奠定基础；根据感知到的铰接车周围的巷道环境及铰接车在地下巷道的行驶路径，完成反应式导航过程中的分段式参考路径规划，其中，得到的分段式参考路径为基于铰接车车身坐标系的分段直线；根据规划完成的分段式参考路径，以优化铰接车对分段式参考路径的跟踪效果以及控制量输出的平顺性为目标，确定非线性模型预测控制的优化函数，得到最优控制序列，以便铰接车对规划完成的分段式参考路径进行跟踪，能够有效提高路径跟踪的精度。

为了更好地理解本发明实施例所述的地下无人铰接车的反应式导航方法，对其进行详细说明：

S101，环境感知：感知铰接车周围的巷道环境；

本实施例中，环境感知主要的传感器为工业激光雷达，例如，威列登(Velodyne)16线激光雷达，通过激光雷达去识别巷道壁、铰接车周围环境障碍物的长、宽、高等信息，激光雷达识别信息后，利用同步定位与地图构建(simultaneous localization and mapping)技术进行巷道环境的地图建立，并在导航过程中使用激光雷达进行巷道测距，从而达到巷道环境感知的目的，在得到地图后，铰接车可根据激光雷达信息进行定位与规划，奠定局部路径决策(S102)的基础，是车辆能够成功完成自主导航的前提，与局部路径决策和运动控制组成完整的地下无人铰接车的反应式导航方法，能够解决同类型控制系统对巷道环境识别效果不佳，大多只能对单个、独立场景进行分析，难以成功地进行动态感知并与整个反应式导航系统进行协调工作等问题。

本实施例中，激光雷达传输的数据经过转换得到极坐标系下的角度和距离，可以表示为L＝(ρ，θ)，其中，ρ为巷道环境目标点在极坐标系下距原点的距离，θ是巷道环境目标点在极坐标系下的角度。在自主导航系统中，为了坐标系的统一及数据的更好处理，需要将极坐标系下的角度和距离转换为直角坐标系下的坐标，其中x为直角坐标系下的横坐标值，y为纵坐标值，公式为：

S102，局部路径决策：根据感知到的铰接车周围的巷道环境及铰接车在地下巷道的行驶路径，完成反应式导航过程中的分段式参考路径规划，其中，得到的分段式参考路径为基于铰接车车身坐标系的分段直线；

本实施例中，反应式导航不需要全局定位与地图信息，所以地下巷道不存在全局坐标系。根据地下实际巷道情况将参考单侧巷道壁改成两侧巷道壁，并以车身坐标系作为基准，在地下实际巷道中用激光雷达将巷道壁扫描点滤波、拟合为多段直线，如图2、图3所示，激光雷达P的水平扫描范围为190°，在地下实际巷道中可用激光雷达将巷道壁扫描点滤波、拟合为多段直线，左侧巷道壁拟合直线为l₁，右侧巷道壁拟合直线为l₂，并且可以得到l₁、l₂拟合直线相对于车身坐标系的方程，并随车身运动实时改变。如图3、图4所示，弯道前设置的RFID射频识别系统可以将弯道信息传递给车辆，其中包括转弯后的左巷道拟合直线l₃、右巷道拟合直线l₄与转弯前的直线l₁、l₂的几何关系，为局部参考路径的预测奠定基础。

本实施例中，所述局部路径决策是一种分段式参考路径决策策略，可以根据感知到的铰接车周围的巷道环境及铰接车在地下巷道的行驶路径，依据激光雷达与RFID射频识别系统拟合的基于车身坐标系的巷道壁直线方程将局部路径策略分为四个阶段，其中包括：

1)直巷道行走阶段。在曲率没有发生较大变化的直线巷道中，如图2、图5所示，其中l₁、l₂两拟合直线平行，可根据铰接车左侧巷道壁拟合直线l₁铰接车右侧巷道壁拟合直线l₂相对于铰接车车身坐标系的直线方程来求解巷道中心线a₁方程，其中，l₁、l₂满足：

其中，k₁、k₂分别为直线l₁、l₂的斜率，c₁、c₂分别为直线l₁、l₂在车身坐标系y轴上的截距。

已知k₁＝k₂，根据上述拟合直线方程组可求出巷道中心线a₁基于车身坐标系方程为：

2)进入弯道前阶段。在识别到RFID给出的弯道信息即进入弯道前，如图3、图4所示，为了提高铰接车转弯时的安全裕度，可将反应式导航策略在“中分线”基础上进行优化，在临近弯道处实行偏离“可能发生碰撞的一侧”的策略，同时考虑到大多地下铰接车宽度小于4m，将参考路径a₂设置为外侧巷道与车身中心的安全距离为预设值t(其中，t＝2m)的直线。根据实际作业情况，可将本阶段分为两种情况，分别为图3所示的左向转弯巷道和图4所示的右向转弯巷道。在左转时，可将参考路径a₂设置为远离铰接车右侧巷道壁2m的平行直线以防止转向与内侧巷道刮蹭，如图5所示，参考路径a₂可根据如下公式得到：

式中，θ₁为k₁的反正切值。

如图4所示，在车辆右转时，可将参考路径a₂设置为远离左侧巷道壁2m的平行直线，同理参考路径a₂方程为：

3)进入弯道后阶段。由于激光雷达识别能力有限，此时RFID会将转弯后的巷道信息如左侧巷道拟合直线l₃、右侧巷道拟合直线l₄相对于转弯前巷道直线l₁、l₂的几何关系进行补充，从而可得到:

式中，k₃、k₄分别为直线l₃、l₄在车身坐标系的斜率，c₃、c₄分别为直线l₃、l₄在车身坐标系y轴上的截距。

若拟合直线l₃、l₄与车身坐标系x轴垂直，则：

其中，c₅、c₆分别表示l₃、l₄在x轴的坐标值；

如图3、图4所示，k₃＝k₄，且l₁与l₃垂直、l₂与l₄垂直，即k₁k₃＝-1、k₂k₄＝-1，为防止转弯后与外侧巷道壁刮蹭，可对局部参考路径进行调整，本阶段也分为左转和右转两种情况。左转时，可将转弯后参考路径a₃设置为远离左侧巷道壁2m的平行直线，已知：

其中，θ₃为k₃的反正切值，表示x轴与直线l₃的夹角；

由于铰接车行走时巷道壁基于车身坐标系的方程是不断变化的，拟合直线相对于车身坐标系的位置不同，如图6所示。因此可根据l₃、l₄两直线在y轴上的截距大小关系求取参考路径直线a₃：

若l₃、l₄与车身坐标系x轴垂直，则参考路径a₃为：

x＝c₅+2

如图4所示，右转时，可将转弯后的参考路径a₃设置为远离右侧巷道壁2m的平行直线，与左转同理可得参考路径a₃为：

若l₃、l₄与车身坐标系x轴垂直，则参考路径a₃为：

x＝c₆+2

根据以上公式，在得到a₂、a₃路径后，可以求取两条直线的交点B，如图3、图4所示，B点即为参考路径转折点。

4)转弯完成阶段。如图3、图4所示，在完成上述阶段后，到达RDID识别区，根据RFID及激光雷达扫描信息获知转弯完成，再次进入直巷道行走阶段，参考路径为巷道中心线，即：

若l₃、l₄与车身坐标系x轴垂直，则巷道中心线为：

在第四阶段完成后，铰接车再次进入直巷道阶段，如此循环，以完成反应式导航过程中的分段式参考路径规划。

本实施例中，得到的分段式参考路径为基于车身坐标系的分段直线，其中，在直巷道中沿中心线行走，在弯道行驶中根据地下巷道环境对参考路径进行动态调整，最后将各个阶段下的参考路径信息结合，得到铰接车需要跟踪的参考路径，该参考路径作为输入信息传递给非线性模型预测控制的优化函数以进行参考路径的跟踪。

S103，运动控制：根据规划完成的分段式参考路径，以优化铰接车对分段式参考路径的跟踪效果以及控制量输出的平顺性为目标，确定非线性模型预测控制的优化函数，得到最优控制序列，以便铰接车对规划完成的分段式参考路径进行跟踪，具体可以包括以下步骤：

A1，建立铰接车的运动学模型；

本实施例中，铰接车在车身坐标系中的运动特征与在全局坐标系中的一致，以前车体车桥中心为车辆控制点，如图7所示，铰接车的前车身和后车身被认为是刚体，轮胎被忽略，图7中，O_f、O_r为前车体、后车体的车桥中心，d为前车体的宽度；铰接角γ可以表示为：

γ＝θ_f-θ_r

其中，建立的铰接车的运动学模型表示为：

其中，v_f、v_r分别为前车体、后车体的速度；γ为铰接角；w_f、w_r分别为前车体、后车体的横摆角速度；L_f、L_r分别为前车体、后车体的长度；

w_γ都表示铰接角速度。

本实施例中，铰接车的状态空间模型为：

其中，

即w_f为前车体横摆角速度，rad/s；

分别为前车体在X轴与Y轴方向的速度分量；θ_f、θ_r分别为前车体、后车体的航向角；θ_f为前车体航向，rad。

A2，对建立的铰接车的运动学模型进行离散化，建立非线性的预测模型，预测模型根据当前时刻的状态量和控制量预测未来时刻的铰接车的位姿信息，具体可以包括以下步骤：

确定铰接车的运动学模型的状态量x与控制量u，x＝[x_f y_f θ_f γ]，u＝[v_f w_γ]，其中，x_f、y_f分别表示前车体在X轴、Y轴方向上的位移，θ_f为前车体的航向角；

将状态量x与控制量u之间的关系描述为

其中，

表示状态量x的微分，

表示x、u与

的函数关系；

利用欧拉法(Euler Method)对建立的运动学模型进行离散化，建立非线性的预测模型，预测模型根据当前时刻的状态量和控制量预测未来一段时间[t,t+N_p]内铰接车的状态量x，所述预测模型表示为：

其中，T表示采样间隔；x(t+i|t)为t时刻第i个预测点的状态量，i＝0,1,...,N_p；u(t+m|t)为t时刻第m个预测点的控制量，m＝0,1,...,N_c；N_p为预测时域，N_c表示控制时域。

A3，将分段式参考路径根据预测步长离散为连续的点，得到由点序列组成的参考路径；

A4，基于得到的预测时域内铰接车的位姿信息和得到的由点序列组成的参考路径，以优化铰接车对分段式参考路径的跟踪效果以及控制量输出的平顺性为目标，确定非线性模型预测控制的优化函数，得到最优控制序列，以便铰接车对规划完成的分段式参考路径进行跟踪。

本实施例中，在铰接车的预测模型中，车辆未来的控制输出量是位置，这就需要设定代价函数目标，对预测后的车辆状态进行评价，从而求得反应式导航下一时刻的最优控制量，具体步骤如下：

进一步地，将铰接车的预测位置与参考轨迹的误差记为e(t)：

e(t+i|t)＝x(t+i|t)-xref(t+i|t)

其中，e(t)是指t时刻的跟踪误差，e(t+i|t)表示在t时刻第i个预测点的跟踪误差，x_ref(t+i|t)表示t时刻第i个预测点参考路径。

本实施例中，为了使控制量变化平稳，加入控制变量这一约束：

Δu(t|t)＝u(t|t)-u(t-1|t)

其中，Δu(t|t)指t时刻的控制增量，u(t-1|t)为上一采样时刻的控制输出量。

本实施例中，由此将非线性模型预测控制的优化函数设定为：

其中，e(t)是指t时刻的跟踪误差，Δu(t)、u(t)分别指t时刻的控制增量和控制量，Q和R都表示权重矩阵，‖·‖表示范数，ρ为权重系数，ε为松弛因子；Np为预测时域；Nc为控制时域；Δu(t+i|t)＝u(t+i|t)-u(t|t)，Δu(t+i|t)为t时刻第i个预测点的控制增量，u(t+i|t)为t时刻第i个预测点的控制量，控制量u＝[v_fw_γ]；

优化函数J中的第一项体现了铰接车对分段式参考轨迹的跟踪效果，第二项则反映了控制量输出的平顺性。

在无人车辆的实际控制中，经常需要考虑车辆的状态量及控制量的一些约束，如(v_fmin,v_fmax)，(ω_γmin,ω_γmax)，(γ_min,γ_max)，其中，v_fmin、v_fmax分别表示v_f的最小值、最大值；w_γmin、w_γmax分别表示w_γ的最小值、最大值；γ_min、γ_max分别表示γ的最小值、最大值。

本实施例中，还考虑车辆的状态量及控制量的一些约束，因此，非线性模型预测控制的优化函数J就是在预测时域内求解下式中的有约束优化问题：

s.t.

v_fmin≤v_f≤v_fmax

w_γmin≤w_γ≤w_γmax

γ_min≤γ≤γ_max

本实施例中，根据以上约束量对该优化函数进行优化求解，最后得出最优的控制量序列：

本实施例中，为了保证实时性及控制精度，只需将该控制量序列中的第一个控制量作为车辆的控制信息，即：

此时将控制量输入给车辆，在接下来的时域，系统重新开始新的优化公式求解，计算出控制输出量，如此循环，直至完成整个铰接车反应式导航控制过程，这样，能够大大降低跟踪误差，提高跟踪精度。

本发明实施例所述的地下无人铰接车的反应式导航方法可在电脑中进行仿真实验。仿真系统为机器人操作系统(Robot Operating System，ROS)，将整个反应式导航方法进行了联合仿真。仿真铰接车的铲斗容量为2立方，空载质量为12吨，满载质量为16吨。设置铰接车前车体轴为1.48米，后车体轴距为1.5米，宽度为1.8米。为了对反应式导航效果进行验证，使用直巷道与直角弯巷道结合的作业环境作为仿真巷道模型。在铰接车完成从起点到终点的作业流程后，可以对其运动过程的每个时刻的信息进行记录，并提取其状态信息，仿真实验结果如表1所示。

表1仿真实验结果

主要信息	参数
		车身长度	7.8m
车辆宽度	1.8m
		前车体长度	3m
最小离墙距离	0.62m
		最大铰接角	0.54rad
最大铰接角速度	0.139rad/s

实验结果表明，即使是在较为严苛的直角弯道转向时，仍然具有0.62米的安全距离，铰接角速度抖振较少，且最大值为0.139rad/s，较好地被控制在控制量约束范围内，同时对于铰接角信息分析，可以看到铰接角的变化较为平顺，最大值为0.54rad，约31度，这也在铰接角的约束范围内，同时平顺的铰接角变化趋势对于执行机构也是有益的，避免了执行机构的抖振风险。

本发明中的反应式导航方法具有较好的自主行走效果，验证了该反应式导航整体平台的搭建的初步成功，为未来的无人驾驶的整体研究及优化提供了框架及基础平台。

以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明所述原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (10)

1.一种地下无人铰接车的反应式导航方法，其特征在于，包括：

感知铰接车周围的巷道环境；

根据感知到的铰接车周围的巷道环境及铰接车在地下巷道的行驶路径，完成反应式导航过程中的分段式参考路径规划，其中，得到的分段式参考路径为基于铰接车车身坐标系的分段直线；

根据规划完成的分段式参考路径，以优化铰接车对分段式参考路径的跟踪效果以及控制量输出的平顺性为目标，确定非线性模型预测控制的优化函数，得到最优控制序列，以便铰接车对规划完成的分段式参考路径进行跟踪。

2.根据权利要求1所述的地下无人铰接车的反应式导航方法，其特征在于，分段式包括：直巷道行走阶段、进入弯道前阶段、进入弯道后阶段和转弯完成阶段。

3.根据权利要求2所述的地下无人铰接车的反应式导航方法，其特征在于，所述根据感知到的铰接车周围的巷道环境及铰接车在地下巷道的行驶路径，完成反应式导航过程中的分段式参考路径规划包括：

在直巷道行走阶段，根据转弯前的左侧巷道壁拟合直线l₁、右侧巷道壁拟合直线l₂相对于铰接车车身坐标系的直线方程求解巷道中心线a₁的方程，其中，得到的巷道中心线a₁基于铰接车车身坐标系方程为：

其中，巷道中心线a₁为直巷道行走阶段的参考路径，k₁为左侧巷道壁拟合直线l₁的斜率，c₁、c₂分别为直线l₁、l₂在铰接车车身坐标系y轴上的截距，x、y为巷道中心线a₁在铰接车车身坐标系x轴、y轴上的坐标。

4.根据权利要求3所述的地下无人铰接车的反应式导航方法，其特征在于，所述根据感知到的铰接车周围的巷道环境及铰接车在地下巷道的行驶路径，完成反应式导航过程中的分段式参考路径规划包括：

在进入弯道前阶段，将参考路径a₂设置为相应侧巷道与铰接车车身中心的安全距离为预设值的直线；其中，

在铰接车左转时，将参考路径a₂设置为距离铰接车右侧巷道壁预设值的平行直线，其中，参考路径a₂的方程为：

其中，θ₁为k₁的反正切值，t为预设值，k₁为转弯前的左侧巷道壁拟合直线l₁的斜率，c₂为右侧巷道壁拟合直线l₂在铰接车车身坐标系y轴上的截距，x、y为参考路径a₂在铰接车车身坐标系x轴、y轴上的坐标；

在铰接车右转时，将参考路径a₂设置为距离铰接车左侧巷道壁预设值的平行直线，其中，参考路径a₂的方程为：

其中，c₁为左侧巷道壁拟合直线l₁在铰接车车身坐标系y轴上的截距。

5.根据权利要求4所述的地下无人铰接车的反应式导航方法，其特征在于，所述根据感知到的铰接车周围的巷道环境及铰接车在地下巷道的行驶路径，完成反应式导航过程中的分段式参考路径规划包括：

在进入弯道后阶段，将转弯后的参考路径a₃设置为距离相应侧巷道壁预设值的平行直线；

其中，在铰接车左转时，将转弯后参考路径a₃设置为距离铰接车左侧巷道壁预设值的平行直线，其中，参考路径a₃为：

其中，θ₃为k₃的反正切值，k₁为转弯前的左侧巷道壁拟合直线l₁的斜率，k₃为转弯后的左巷道拟合直线l₃的斜率，t为预设值，c₃、c₄分别为转弯后的左巷道拟合直线l₃、转弯后的右巷道拟合直线l₄在铰接车车身坐标系y轴上的截距，x、y为参考路径a₃在铰接车车身坐标系x轴、y轴上的坐标；

其中，t为预设值，x为参考路径a₃在铰接车车身坐标系x轴上的坐标，c₅表示转弯后的右巷道拟合直线l₄在x轴的坐标值；

在铰接车右转时，将转弯后的参考路径a₃设置为距离铰接车右侧巷道壁预设值的平行直线，其中，参考路径a₃为：

6.根据权利要求5所述的地下无人铰接车的反应式导航方法，其特征在于，所述根据感知到的铰接车周围的巷道环境及铰接车在地下巷道的行驶路径，完成反应式导航过程中的分段式参考路径规划包括：

在转弯完成阶段，再次进入直巷道行走阶段，参考路径为巷道中心线。

7.根据权利要求1所述的地下无人铰接车的反应式导航方法，其特征在于，所述根据规划完成的分段式参考路径，以优化铰接车对分段式参考路径的跟踪效果以及控制量输出的平顺性为目标，确定非线性模型预测控制的优化函数，得到最优控制序列，以便铰接车对规划完成的分段式参考路径进行跟踪包括：

建立铰接车的运动学模型；

对建立的铰接车的运动学模型进行离散化，建立非线性的预测模型，预测模型根据当前时刻的状态量和控制量预测未来时刻的铰接车的位姿信息；

将分段式参考路径根据预测步长离散为连续的点，得到由点序列组成的参考路径；

基于得到的预测时域内铰接车的位姿信息和得到的由点序列组成的参考路径，以优化铰接车对分段式参考路径的跟踪效果以及控制量输出的平顺性为目标，确定非线性模型预测控制的优化函数，得到最优控制序列，以便铰接车对规划完成的分段式参考路径进行跟踪。

8.根据权利要求7所述的地下无人铰接车的反应式导航方法，其特征在于，建立的铰接车的运动学模型表示为：

其中，v_f、v_r分别为前车体、后车体的速度；γ为铰接角；w_f、w_r分别为前车体、后车体的横摆角速度；L_f、L_r分别为前车体、后车体的长度；

w_γ都表示铰接角速度。

9.根据权利要求8所述的地下无人铰接车的反应式导航方法，其特征在于，确定的非线性模型预测控制的优化函数J表示为：

其中，e(t)是指t时刻的跟踪误差，Δu(t)、u(t)分别指t时刻的控制增量和控制量，Q和R都表示权重矩阵，‖·‖表示范数，ρ为权重系数，ε为松弛因子；Np为预测时域；Nc为控制时域；e(t+i|t)＝x(t+i|t)-x_ref(t+i|t)，e(t+i|t)表示在t时刻第i个预测点的跟踪误差，x(t+i|t)为t时刻第i个预测点的状态量，状态量x＝[x_f y_f θ_f γ]，x_f、y_f分别表示前车体在X轴、Y轴方向上的位移，θ_f为前车体的航向角，x_ref(t+i|t)表示t时刻第i个预测点参考路径；Δu(t+i|t)＝u(t+i|t)-u(t|t)，Δu(t+i|t)为t时刻第i个预测点的控制增量，u(t+i|t)为t时刻第i个预测点的控制量，控制量u＝[v_f w_γ]；

确定的非线性模型预测控制的优化函数J的约束条件包括：

s.t.

v_fmin≤v_f≤v_fmax

w_γmin≤w_γ≤w_γmax

γ_min≤γ≤γ_max

其中，s.t.表示约束条件；v_fmin、v_fmax分别表示v_f的最小值、最大值；w_γmin、w_γmax分别表示w_γ的最小值、最大值；γ_min、γ_max分别表示γ的最小值、最大值。

10.根据权利要求9所述的地下无人铰接车的反应式导航方法，其特征在于，最优控制序列

其中，

为t时刻第1个最优控制量，将该最优控制序列中第一个元素

作为铰接车的实际控制量控制所述铰接车对规划完成的分段式参考路径进行跟踪。

Images