CN111142520A - 一种履带式无人车辆在断崖障碍道路上的规划与控制方法 - Google Patents

Abstract

本发明提供了一种履带式无人车辆在断崖障碍道路上的规划与控制方法，包括在具有断崖的道路上的路径规划、对断崖障碍数据信息的处理、攀爬复杂形状的断崖时的步态规划与控制。在履带式无人车辆上加入路径规划系统，当车辆行驶时遇到断崖时，车辆规划出较优的路径发送给路径跟踪系统；对检测出的断崖尺寸数据进行计算与判断，通过设计的代价函数选择出履带式无人车辆的攀爬断崖的工作段；根据断崖的尺寸数据以及车辆的尺寸数据，计算并设计出控制时序与控制方法，使得在履带式无人车辆能够攀爬高度变化的复杂断崖。

Description

一种履带式无人车辆在断崖障碍道路上的规划与控制方法

技术领域

本发明涉及无人车辆的规划与控制技术领域，具体涉及一种用于履带式无人车辆在断崖障碍道路上的规划与控制方法。

背景技术

近年来，履带式无人车辆在火场灭火、深入地震区域救援以及战场作战等场合的应用越来越广泛。但各种复杂的环境对履带式无人车辆的智能化程度、地形的通过性等提出更高的要求，因此许多履带式无人车辆的新型结构以及新型的规划方法被提出。例如专利申请号为“CN201610241122.9”的发明专利“一种高机动高适应性地面无人车辆”，提出一种四边形六足履式结构，以提升车辆的越野能力；并且专利申请号为“CN201710350810.3”的发明专利“一种六足履式无人地面车辆的上台阶步态规划系统及其方法”，提出了一种无人车辆上台阶的步态规划方法，能使车辆具备自主翻越台阶的能力。但是该方法所能爬上的台阶的形状较为理想化，不适用于翻越野外环境道路上高度变化的复杂断崖；且该方法中判断能否爬上台阶的决策条件较为单一，没有考虑到台阶的各种尺寸参数对通过性的影响；而且该方法的智能化程度较低，当遇到无法翻越的状况时，车辆只能停车报警，无法满足实际越野的需求。

发明内容

本发明为解决上述现有技术存在的不足之处，提出一种履带式无人车辆在断崖障碍道路上的规划与控制方法，以期能使车辆具备自主在断崖障碍道路上正常行驶通过的能力，进而满足实际越野环境。

一种履带式无人车辆在断崖障碍道路上的规划与控制方法，其特征在于，包括如下步骤：

步骤1、设定车辆所需到达的目标点，规划期望路径并沿着该期望路径行驶，实时根据路况更新期望路径；

步骤2、当检测到路径前有断崖障碍时，使车辆在距断崖一定距离前停车；

步骤3、确定初始工作段，在该初始工作段内，建立代价函数，选择出代价函数的最小值对应的区域，作为车辆攀爬断崖的最终工作段；

步骤4、将最终工作段的断崖前设定为目标点，规划最终局部期望路径，车辆沿该最终局部期望路径运动到目标点；

步骤5、计算车辆行走机构的步态角，车辆向前移动距离，完成翻越断崖的全过程。

由以上技术方案可知，本发明具有如下有益效果：

1、本发明中履带式无人车辆加入了对断崖障碍宽度信息数据的处理判断，当此时规划路径上的断崖宽度小于某数值时，履带式无人车辆放弃翻越该断崖，选择从断崖侧方绕过，节省了大量的时间和能量消耗，提高了履带式无人车辆的效率。

2、本发明中详细介绍了履带式无人车辆对前方的断崖障碍物高度数据的处理方法，判断车辆能否翻越前方断崖障碍，并在此过程中使用代价函数来选取无人车辆在断崖上的翻越工作段，使得无人车辆对野外环境的适应性和断崖的翻越效率更高。

3、本发明加入了路径规划系统，当履带式无人车辆判断出前方断崖障碍不能翻越通过后，车辆会进行局部路径规划生成一条新的路径，避免了现有技术中车辆停车报警情况的出现，使履带式无人车辆在新的路径上继续行驶，最终到达目标点。

4、本发明使用的履带模型最大的还原实际使用的履带，不再简化为四边形结构，更为精确的尺寸数据，使得步态旋转角等控制量的计算更加精确。

5、本发明给出了一种当左右履带轮攀爬的断崖高度不一致时步态控制方法，使得车辆能够通过更为复杂的断崖地形，提高了车辆的通过性能，并给出了各个时刻的步态角度的计算方法。

附图说明

图1为本发明中履带式无人车辆上台阶过程t₁时刻示意图；

图2为本发明中履带式无人车辆上台阶过程t₂时刻示意图；

图3为本发明中履带式无人车辆上台阶过程t₃时刻示意图；

图4为本发明中履带式无人车辆上台阶过程t₄时刻示意图；

图5为本发明中履带式无人车辆上台阶过程t₇时刻示意图；

图6为本发明中履带式无人车辆上台阶过程t₈时刻示意图；

图7为本发明中履带式无人车辆上台阶过程t₉时刻示意图；

图8为本发明使用的履带式无人车辆的结构示意图；

图9为本发明的计算过程中所使用前行走机构的履带结构简化示意图；

图10为本发明的计算过程中所使用中行走机构的履带结构简化示意图；

图11为本发明的行走机构的正视图；

图12为本发明的行走机构的侧视图；

图13为本发明的方法流程图。

具体实施方式

下面结合附图对本发明的一种优选实施方式作详细的说明。

如图8、11和12所示，本发明使用的履带式无人车辆包括车体，以及通过驱动机构驱动的左右行走机构。本实施例中，六足履式无人车辆为专利申请号为“CN201610241122.9”的发明专利“一种高机动高适应性地面无人车辆”中车体结构，包括车体和六个足履式行走机构，该行走机构位于车辆的两侧，左右各三个成对称布置，构成六履带式车辆结构。左右行走机构分别为左前行走机构、右前行走机构、左中行走机构、右中行走机构、左后行走机构和右后行走机构。每一个行走机构中都包括一个驱动轮以及两侧通过履带传动连接的翼轮。

如图13所示，本发明提供一种履带式无人车辆在断崖障碍道路上的规划与控制方法，包括如下步骤：

S1、设定车辆所需到达的目标点，规划期望路径并沿着该期望路径行驶，实时根据路况更新期望路径；

S2、当检测到路径前有断崖障碍时，使车辆在距断崖一定距离前停车；

S3、确定初始工作段，在该初始工作段内，建立代价函数，选择出代价函数的最小值对应的区域，作为车辆攀爬断崖的最终工作段；

S4、将最终工作段的断崖前设定为目标点，规划最终局部期望路径，车辆沿该最终局部期望路径运动到目标点；

S5、规划并计算出车辆行走机构在各时段的步态角度和车辆前移距离，使车辆翻越断崖。

本实施例中，更加详细的步骤如下：

步骤1、履带式无人车辆开始工作，设定车辆所需到达的目标点，车辆规划出一条期望路径，并实时根据路况更新期望路径；

步骤2、履带式无人车辆沿着该期望路径行驶，当激光雷达检测到路径前有断崖障碍时，便测量出车辆距断崖的距离以及断崖的尺寸数据信息，此时车辆中的控制器根据测量出的车辆到断崖的距离，计算出车辆所需的制动量并发送给执行机构，使车辆在距断崖一定距离前停车；

步骤3、选择两个满足式(1)的高度点之间的断崖为初始工作段，初始工作段长度记为b₁；

h＜K_HH₀ (1)

式(1)中，h为激光雷达检测出来的前方断崖的高度；H₀为履带式无人车辆的最小离地间隙；K_H为履带式无人车辆的障碍物通过安全系数；

步骤4、根据选择出的初始工作段长度b₁，使用式(2)进行判断，若判断不成立，则履带式无人车辆规划出一条直接绕过断崖的期望路径，返回步骤2；

b₁＞K_wB (2)

式(2)中，B为履带式无人车辆的横向宽度；K_w为断崖宽度判断系数；

步骤5、在选择出的断崖工作段内，以车辆的前进方向为视角，从左至右以B_win作为窗口宽度，以履带宽度b作为窗口的步进长度在工作段内做遍历，得到N组数据；

B_win＝K_winB (3)

式(3)中，K_win为窗口宽度系数；

步骤6、控制器计算出每个窗口内部高度的最大值

每个窗口内部高度的最大变化值{Δh¹,Δh²...Δh^N}、每个窗口的高度平均值{h¹,h²...h^N}和每个窗口的高度方差{S¹,S²...S^N}；

步骤7、使用式(4)和式(5)对每个窗口内部高度的最大值和每个窗口内部高度的最大变化值做判断，若N组数据都判断不成立，则放弃该条路径，重新进行局部路径规划，得到新的一条期望路径，返回步骤2；

Δhⁱ/B_win＜α₀ i＝1...N (5)

式(4)中，h₀为履带式无人车辆的所能攀爬的最大断崖高度，由式(6)计算；

式(5)中，α₀为履带式无人车辆的所能攀爬的最大断崖侧倾坡度；

h₀＝K_L(L₁+r₁) (6)

式(6)中，K_L为履带式无人车辆的断崖高度安全系数、L₁为两翼轮中心的距离，r₁为一侧翼轮圆心至其履带外侧圆弧的半径；

步骤8、建立代价函数J，满足步骤7的组数设为k，使用公式(7)计算出每组的代价函数值J_i，从中对选择出最小值J_min对应的窗口区域，作为车辆最终的攀爬断崖的最终工作段；

J_i＝α₁hⁱ+α₂Sⁱ i＝1...k (7)

式(7)中，α₁为断崖高度权值，α₂为断崖高度方差的权值；

步骤9、将选取的最终工作段的断崖前的距离l₂处的一点设定为目标点，l₂的选择满足应使式(8)，并且车辆在该目标点处的航向与对应的台阶面垂直，履带式无人车辆以当前点作为起始点，规划出最终局部期望路径，车辆沿该路径运动到目标点；

式(8)中，R₁为驱动轮中心至其所配合的履带外侧的半径；β₁为履带对驱动轮一侧的包角；β₂为履带对一侧翼轮的包角；δ₁为车辆步态调整后车辆与断崖间的安全距离，δ₁为一个常数，其取值在保证履带式无人车辆不与断崖碰撞的前提下应尽可能较小，具体数值大小由试验确定；

步骤10、激光雷达测量出前方的断崖高度信息，履带式无人车辆从中读取出左右行走机构所对应的前方断崖高度H_l和H_r；

参照图1-7，计算车辆行走机构的步态角，车辆向前移动距离，完成翻越断崖的全过程：

步骤11、在t₁初始时刻控制器发出控制指令，使控制步态电机产生如(9)所示的步态角；

式(9)中，以顺时针方向为正，

表示此刻左前行走机构的步态角；

表示此刻右前行走机构的步态角；

表示此刻左中行走机构的步态角；

表示此刻右中行走机构的步态角；

表示此刻左后行走机构的步态角；

表示此刻右后行走机构的步态角，下文中不同时刻的步态表示方法与此类似；

步骤12、控制器使用式(10)计算履带式无人车辆距台阶的距离l₃；

由式(11)计算出完成该步骤后的时刻t₂；

式(11)中，w₁表示t₁时刻至t₂时刻的步态机构的旋转角速度；

步骤13、在t₂时刻，控制器利用式(12)计算各行走机构的步态角；

由式(13)计算该步骤完成后所达到时刻t₃；

式(13)中，w₂表示t₂时刻至t₃时刻的步态机构的旋转角速度；

步骤14、使用式(14)计算前驱动轮中心至断崖面的水平距离l₆；

式(14)中θ₃表示为计算代换角度1，在图9中表示为直线BO₁与直线AO₁的夹角∠AO₁B；θ₄表示为计算代换角度2，在图9中表示为直线AB与直线AO₁的夹角∠BAO₁；它们的数值由式(15)计算得到，半径r₂表示为前驱动轮圆心至断崖边距离；

使用式(16)最终计算出中驱动轮中心至断崖面的水平距离l₇；

使用式(17)计算出中驱动轮中心至地平面的高度h₇；

步骤15、将左右断崖高度H_l和H_r带入步骤14中分别计算出左右行走机构的

和

由式(18)计算出中驱动轮中心至断崖垂面的最小距离；

步骤16、控制器向6台驱动电机发出控制指令，使各个驱动轮转动，足履式无人车辆以速度v向前移动，使用式(19)计算出向前运动完成后的时刻t₄；

步骤17、到达t₄时刻，向中行走机构的步态电机发出控制指令，控制步态电机产生式(20)所示的步态角，同时在t₅至t₆时刻向六组驱动机构的驱动电机发送控制指令，使主驱动轮转动，履带式无人车辆向前运动；

其中t₅至t₆是在t₄至t₇时刻内的一段范围，其具体的时刻由试验确定；

由式(21)计算该步骤后所达到时刻t₇；

步骤18、行走机构的运动相对于驱动机构是相互独立，使用式(22)计算在步态机构的单独作用下，中驱动轮中心至台阶垂直面的水平距离

式(23)中θ₅表示为计算代换角度3，在图10中表示为直线DO₃与直线CO₃的夹角∠CO₃D；θ₆表示为计算代换角度4，在图10中表示为直线CD与直线CO₃的夹角∠DCO₃；r₃表示为中驱动轮圆心至断崖边距离，其数值分别下面的式子计算得到；

步骤19、对系统运行做简化处理，使用式(24)计算在驱动机构的单独作用下中驱动轮行驶的水平距离

式(24)中，n为驱动轮转速；

步骤20、由运动学关系可知

是超前于断崖垂面，

是滞后于断崖垂面，使用式(25)计算出中驱动轮圆心与断崖垂面的水平距离l₁₀；

步骤21、到达t₇时刻，控制器向步态电机发送控制指令，产生式(26)所示的步态角；

使用式(27)计算该步骤完成后所达到时刻t₈；

步骤22、到达t₈时刻，控制器向6个驱动电机发送控制指令，使车辆以速度v向前移动距离l₁₁，最终使车辆在t₉时刻完成翻越断崖的全过程。

式(28)中l₁₁>L₀，具体数值大小由前方道路情况确定。

以上所述实施方式仅仅是对本发明的优选实施方式进行描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案作出的各种变形和改进，均应落入本发明的权利要求书确定的保护范围内。

Claims (5)

1.一种履带式无人车辆在断崖障碍道路上的规划与控制方法，其特征在于，包括如下步骤：

步骤1、设定车辆所需到达的目标点，规划期望路径并沿着该期望路径行驶，实时根据路况更新期望路径；

步骤2、当检测到路径前有断崖障碍时，使车辆在距断崖一定距离前停车；

步骤3、确定初始工作段，在该初始工作段内，建立代价函数，选择出代价函数的最小值对应的区域，作为车辆攀爬断崖的最终工作段；

步骤4、将最终工作段的断崖前设定为目标点，规划最终局部期望路径，车辆沿该最终局部期望路径运动到目标点；

步骤5、规划并计算出车辆行走机构在各时段的步态角度和车辆前移距离，使车辆翻越断崖。

2.根据权利要求1所述的规划与控制方法，其特征在于，所述步骤3中，确定初始工作段的具体方法为：

步骤31、选择两个满足式(1)的高度点之间的断崖为初始工作段，初始工作段长度记为b₁；

h＜K_HH₀ (1)

其中，h为前方断崖的高度，H₀为车辆的最小离地间隙，K_H为车辆的障碍物通过安全系数；

步骤32、使用式(2)进行判断，若不成立，则规划出直接绕过断崖的期望路径，并返回至步骤2；

b₁＞K_wB (2)

其中，B为车辆横向宽度，K_w为断崖宽度判断系数。

3.根据权利要求1所述的规划与控制方法，其特征在于，所述步骤3中，建立代价函数，选择出代价函数的最小值对应的区域，作为车辆攀爬断崖的最终工作段，具体方法为：

步骤33、在初始工作段内，以车辆的前进方向为视角，从左至右以B_win作为窗口宽度，以车辆的履带宽度b作为窗口的步进长度在初始工作段内做遍历，得到的数据的组数记为N：

B_win＝K_winB (3)

其中，K_win为窗口宽度系数；

步骤34、计算出每个窗口内部高度的最大值

每个窗口内部高度的最大变化值{Δh¹,Δh²...Δh^N}、每个窗口的高度平均值{h¹,h²...h^N}和每个窗口的高度方差{S¹,S²...S^N}；

步骤35、使用式(4)和式(5)对每个窗口内部高度的最大值和每个窗口内部高度的最大变化值做判断，若N组数据都判断不成立，则放弃该条路径，重新进行局部路径规划，得到新的期望路径，并返回步骤2执行；若成立，则执行步骤36；

Δhⁱ/B_win＜α₀ i＝1...N (5)

其中，h₀为车辆的所能攀爬的最大断崖高度，其由式(6)计算；α₀为车辆的所能攀爬的最大断崖侧倾坡度；

h₀＝K_L(L₁+r₁) (6)

其中，K_L为车辆的断崖高度安全系数，L₁为车辆两翼轮中心的距离，r₁为一侧翼轮圆心至其履带外侧圆弧的半径；

步骤36、建立代价函数J_i，满足步骤35的组数设为k，使用公式(7)计算出每组的代价函数值，选择出最小值J_min对应的窗口区域，作为车辆攀爬断崖的最终工作段；

J_i＝α₁hⁱ+α₂Sⁱ i＝1...k (7)

其中，α₁为断崖高度权值，α₂为断崖高度方差的权值。

4.根据权利要求1所述的规划与控制方法，其特征在于，所述步骤4中，将最终工作段的断崖前的距离l₂处点设定为目标点，规划最终局部期望路径，车辆沿该最终局部期望路径运动到目标点，具体方法为：

将最终工作段朝向车辆延伸距离l₂处的点设定为目标点，l₂的选择满足式(8)，并且车辆在该目标点处的航向与对应的断崖面垂直，以当前点作为起始点，规划出最终局部期望路径，车辆沿该路径运动到目标点；

其中，R₁为驱动轮中心至其所配合的车辆履带外侧的半径；β₁为车辆履带对驱动轮一侧的包角；β₂为车辆履带对一侧翼轮的包角；δ₁为车辆步态调整后车辆与断崖间的安全距离。

5.根据权利要求1所述的规划与控制方法，其特征在于，所述步骤5中，计算车辆行走机构的步态角，车辆向前移动距离，完成翻越断崖的全过程，具体方法为：

步骤501、测量前方的断崖高度信息，车辆从中读取出左右行走机构所对应的前方断崖高度H_l和H_r；

步骤502、在t₁初始时刻发出控制指令，控制步态电机产生如(9)所示的步态角；

其中，以顺时针方向为正，

表示此刻左前行走机构的步态角，

表示此刻右前行走机构的步态角，

表示此刻左中行走机构的步态角，

表示此刻右中行走机构的步态角，

表示此刻左后行走机构的步态角，

表示此刻右后行走机构的步态角；

步骤503、计算车辆距台阶的距离l₃，公式如下：

计算完成该步骤后的时刻t₂：

其中，w₁为t₁时刻至t₂时刻的步态机构的旋转角速度；

步骤504、在t₂时刻，计算各行走机构的步态角：

计算该步骤完成后所达到时刻t₃：

其中，w₂为t₂时刻至t₃时刻的步态机构的旋转角速度；

步骤505、计算车辆前驱动轮中心至断崖面的水平距离l₆：

其中，θ₃表示为计算代换角度1，θ₄表示为计算代换角度2，r₂为表示为前驱动轮圆心至断崖边距离；

计算车辆中驱动轮中心至断崖面的水平距离l₇：

计算车辆中驱动轮中心至地平面的高度h₇：

步骤506、将左右断崖高度H_l和H_r带入步骤14中分别计算出左右行走机构的

和

由式(18)计算出车辆中驱动轮中心至断崖垂面的最小距离；

步骤507、车辆以速度v向前移动，计算向前运动完成后的时刻t₄：

步骤508、到达t₄时刻，向中行走机构的步态电机发出控制指令，控制步态电机产生式(20)所示的步态角，同时在t₅至t₆时刻车辆向前运动，其中t₅至t₆为t₄至t₇时刻内的一段范围；

计算该步骤后所达到时刻t₇：

步骤509、计算在步态机构的单独作用下，车辆中驱动轮中心至台阶垂直面的水平距离

其中，θ₅表示为计算代换角度3，θ₆表示为计算代换角度4，r₃表示为中驱动轮圆心至断崖边距离；

步骤510、计算在驱动机构的单独作用下车辆中驱动轮行驶的水平距离

其中，n为驱动轮转速，R₁为驱动轮中心至其所配合的车辆履带外侧的半径；

步骤511、计算车辆中驱动轮圆心与断崖垂面的水平距离l₁₀：

步骤512、到达t₇时刻，产生式(26)所示的步态角；

计算该步骤完成后所达到时刻t₈：

步骤513、到达t₈时刻，车辆以速度v向前移动距离l₁₁，最终使车辆在t₉时刻完成翻越断崖的全过程，

其中，l₁₁>L₀。

Images