一种六足履式无人地面车辆的上台阶步态规划系统及其方法与流程

本发明属于无人车辆运动规划领域，具体涉及一种新型六足履式无人车辆上台阶步态规划系统及其方法。

背景技术：

近年来，用于野外作业的地面无人车辆在战场侦察、反恐作战、救灾救援等领域发挥了越来越大的作用。各种复杂的环境对地面无人车辆的通过性提出更高的要求，传统的轮式或履带式无人车辆已经难以适应这种要求。许多新型结构的地面无人车辆被提出，例如专利申请号为“cn201610615556.0”的发明专利“一种摇臂轮式地面无人车辆”，提出一种六轮摇臂结构，又如专利申请号为“cn201610241122.9”的发明专利“一种高机动高适应性地面无人车辆”，提出一种四边形六足履式结构，以提升车辆的越野能力。但是对于这类结构在控制车辆上台阶时，需由操作者通过遥控器控制每一个步态电机的动作，这使得遥控操作非常复杂，操作者必须经过长期训练才能完成对车辆的控制，从而导致控制操作的效率不高，无法满足实际越野需求。

技术实现要素：

本发明为解决上述现有技术存在的不足之处，提出一种六足履式无人地面车辆的上台阶步态规划系统及其方法，以期能使车辆具备自主翻越台阶的能力，从而降低车辆上台阶操作难度，进而满足实际越野环境。

本发明为达到上述发明目的，采用如下技术方案：

本发明一种应用于六足履式无人地面车辆的上台阶步态规划系统的特点包括：主控制器、激光雷达、无线接收模块和步态电机；

所述激光雷达横向安装在所述六足履式无人地面车辆的顶部；

所述六足履式无人地面车辆正对台阶时，所述无线接收模块接收上台阶步态开始指令并发送给所述主控制器；

所述主控制器根据所述上台阶步态开始指令控制所述激光雷达检测车辆与台阶之间的距离x以及台阶的高度d，从而得到激光雷达数据；

所述主控制器根据所述上台阶步态开始指令和激光雷达数据进行步态规划处理，生成步态序列和步态电机速度，并转换为can报文后发送给所述步态电机；所述步态序列用步态角表示；

所述步态电机根据所述can报文控制车辆完成上台阶动作。

本发明一种六足履式无人地面车辆的上台阶步态规划方法的特点是应用于由主控制器、激光雷达、无线接收模块和步态电机构成的系统中，所述步态规划处理是按如下步骤进行：

步骤1、所述主控制器根据所述上台阶步态开始指令，控制所述步态电机产生如式(1)所示的步态角：

式(1)中，θf表示前足履的步态角；θm表示中足履的步态角；θr表示后足履的步态角；所述步态角定义为平行四边形足履长对角线与车体纵轴线的夹角；φ表示平行四边形足履长对角线与足履边的夹角；

步骤2、所述主控制器控制车辆以恒定速度v向前行驶，并通过所述雷达检测台阶高度d；

步骤3、所述主控制器判断式(2)是否成立，若成立表示车辆能否翻越，并执行步骤4，否则表示不能翻越，并停车报警；

d＞c/2-g(2)

式(2)中，c为所述平行四边形足履长对角线的长度，g表示平行四边形足履高的一半，并有

步骤4、所述主控制器利用式(3)计算所述前足履在t0时刻的步态角

式(3)中，δ表示所述前足履超出所述台阶高度的期望值；

步骤5、所述主控制器判断式(4)是否成立，若成立，则记录当前时刻为t0时刻并启动定时器后，并执行所述步态角否则，继续判断；

式(4)中，为所述前足履在t0时刻的步态角；ω为设定的所述前足履的转速；

步骤6、所述主控制器利用式(5)得到t1时刻：

t1＝x/v+t0(5)

步骤7、所述主控制器利用式(6)计算t1时刻所述中足履的步态角和后足履的步态角所述前足履步态角保持不变：

式(6)中，b为所述六足履式无人地面车辆的轴距；

步骤8、当所述定时器达到t1时刻时，所述主控制器控制所述步态电机执行步态角和

步骤9、所述主控制器利用式(7)计算t2时刻：

式(7)中，h2表示t1时刻前足履位于下方的钝角顶点(a)到所述台阶的水平距离，并由式(8)获得；h4表示中间变量，并由式(9)获得；

式(8)中，h1表示t1时刻所述钝角顶点到所述台阶上部的垂直距离，并由式(10)获得；

式(9)中，h3表示中间变量，并由式(11)获得；

式(10)中，i为平行四边形足履的边长；

式(11)中，φ2表示t2时刻车辆纵轴线与铅垂线的夹角，并由式(12)获得；φ3表示中间变量，并由式(13)获得；

步骤10、所述主控制器利用式(14)计算t2时刻的前足履的步态角所述中足履的步态角和所述后足履的步态角

步骤11、所述定时器达到t2时刻时，所述主控制器控制所述步态电机执行步态角和

步骤12、所述主控制器利用式(15)计算t3时刻：

式(15)中，l表示t0时刻中足履位于下方的钝角顶点与后足履位于下方的钝角顶点之间的距离，并由式(16)获得；

l＝b-2k+2icos2φ(16)

式(16)中，k表示t0时刻中足履中心与中足履位于前方的锐角顶点之间的水平距离，并由式(17)获得；

步骤13、所述主控制器利用式(18)计算t3时刻前足履的步态角所述中足履的步态角及所述后足履的步态角保持不变：

步骤14、所述定时器达到t3时刻时，所述主控制器控制所述步态电机执行步态角

步骤15、所述主控制器按式(19)计算t4时刻：

步骤16、所述主控制器利用式(20)计算t4时刻后足履的步态角所述前足履的步态角及中足履的步态角保持不变：

步骤17、所述定时器达到t4时刻时，所述主控制器控制所述步态电机执行步态角从而完成上台阶动作成。

与已有技术相比，本发明的有益效果是：

1、本发明详细分析了六足履式车辆上台阶过程的各个关键时刻，给出了关键时刻下的步态角度的计算方法，并利用主控制器产生步态序列，控制步态电机动作，从而实现了车辆自主上台阶过程。上台阶过程开始后就无需人工干预，与现有技术方案利用遥控器依靠人工操作上台阶的方式相比，极大的降低了操作难度，提高了车辆在越野环境中的实用性。

2、本发明将六足履式车辆上台阶过程分为若干阶段，计算每个阶段的开始时刻和每个阶段各个步态电机的目标位置，每个阶段内仅仅控制步态电机匀速转动到目标位置，而不控制任意时刻各个步态电机的位置，这样做在达到相同效果的情况下，极大的简化了步态规划过程和电机控制方法。

3、对于上台阶过程中的关键步骤，比如步骤9中对t2时刻的计算，本发明考虑了两个关键时刻之间车辆运动的动态过程对车辆纵轴线倾角以及步态角的影响，这使得计算的结果更加精确。

附图说明

图1为本发明车辆结构示意图(从车辆右侧看去)及各变量示意图；

图2为本发明上台阶过程t0时刻示意图；

图3为本发明上台阶过程t1时刻示意图；

图4为本发明上台阶过程t2时刻示意图；

图5为本发明上台阶过程t3时刻示意图；

图6为本发明上台阶过程t4时刻示意图；

图7为本发明上台阶过程完成示意图；

图8为本发明步骤9计算过程示意图。

具体实施方式

本实施例中，适用的六足履式无人地面车辆为专利申请号为“cn201610241122.9”的发明专利“一种高机动高适应性地面无人车辆”中车体结构，包括车体和六个平行四边形履带轮，本发明中将这种履带轮称为足履，并分为前、中、后三组，每组两个足履；车辆配备遥控器，遥控器能控制车辆前进、后退、左转、右转，并具有上台阶开始按钮，以发送上台阶开始指令；

一种应用于六足履式无人地面车辆的上台阶步态规划系统，适用的环境为一级台阶，而不适用于连续的楼梯，该上台阶步态规系统包括：主控制器、激光雷达、无线接收模块和步态电机；

激光雷达横向安装在六足履式无人地面车辆的顶部，并通过网口与主控制器相连；

操作者通过遥控器控制六足履式无人地面车辆正对台阶，并使车辆前端距离台阶约3～5m，然后按下上台阶开始按钮，无线接收模块接收上台阶步态开始指令并通过串口发送给主控制器；

主控制器根据上台阶步态开始指令控制激光雷达检测车辆与台阶之间的距离x以及台阶的高度d，从而得到激光雷达数据；

主控制器根据上台阶步态开始指令和激光雷达数据进行步态规划处理，生成步态序列和步态电机速度，并转换为can报文后发送给步态电机；步态序列用步态角表示，步态角定义为足履长对角线与车辆纵轴线的夹角，并规定从车辆右侧看去，顺时针旋转为正，逆时针旋转为负，如图1所示；

步态电机根据can报文控制车辆完成上台阶动作。

本实施例中，一种六足履式无人地面车辆的上台阶步态规划方法，应用于由主控制器、激光雷达、无线接收模块和步态电机构成的系统中，该步态规划处理是按如下步骤进行：

步骤1、主控制器根据上台阶步态开始指令，控制步态电机产生如式(1)所示的步态角：

式(1)中，θf表示前足履的步态角；θm表示中足履的步态角；θr表示后足履的步态角；步态角定义为平行四边形足履长对角线与车体纵轴线的夹角；φ表示平行四边形足履长对角线与足履边的夹角；

步骤2、主控制器控制车辆以恒定速度v向前行驶，车速的大小须有试验确定，并通过雷达检测台阶高度d；

步骤3、主控制器判断式(2)是否成立，若成立表示车辆能否翻越，并执行步骤4，否则表示不能翻越，并停车报警；

d＞c/2-g(2)

式(2)中，c为平行四边形足履长对角线的长度，g表示平行四边形足履高的一半，如图2所示，并有

步骤4、主控制器利用式(3)计算前足履在t0时刻的步态角

式(3)中，δ表示前足履超出台阶高度的期望值，如图3所示，该值与车辆尺寸有关，需有试验确定合适的经验值，在本实施例中，取为0.1；

步骤5、主控制器判断式(4)是否成立，若成立，则记录当前时刻为t0时刻并启动定时器后，并执行步态角否则，继续判断；

式(4)中，为前足履在t0时刻的步态角；ω为设定的前足履的转速，转速的大小须有试验确定；

步骤6、主控制器利用式(5)得到t1时刻，t1时刻六足履式无人地面车辆的步态如图3所示：

t1＝x/v+t0(5)

步骤7、主控制器利用式(6)计算t1时刻中足履的步态角和后足履的步态角前足履步态角保持不变：

式(6)中，b为六足履式无人地面车辆的轴距，如图1所示；

步骤8、当定时器达到t1时刻时，主控制器控制步态电机执行步态角和

步骤9、主控制器利用式(7)计算t2时刻，t2时刻六足履式无人地面车辆的步态如图4所示：

式(7)中，h2表示t1时刻前足履位于下方的钝角顶点a到台阶的水平距离，如图3所示，并由式(8)获得；h4表示中间变量，如图8所示，并由式(9)获得；

式(8)中，h1表示t1时刻钝角顶点a到台阶上部的垂直距离，如图3所示，并由式(10)获得；

式(9)中，h3表示中间变量，如图8，并由式(11)获得；

式(10)中，i为平行四边形足履的边长，如图1所示；

式(11)中，φ2表示t2时刻车辆纵轴线与铅垂线的夹角，并由式(12)获得；φ3表示中间变量，并由式(13)获得；

对以上计算简单解释如下：在钝角顶点a从图3位置运动到图4所示位置的过程中，车辆纵轴线的运动可以看成：首先将车辆纵轴线向前平移h2，再向上平移h1，再以前足履中心为圆心旋转一定角度，使得后足履中心距地面高度为g，如图8所示，图8中标出了h3和h4，而φ3为车辆纵轴线旋转前与铅锤线夹角。

步骤10、主控制器利用式(14)计算t2时刻的前足履的步态角中足履的步态角和后足履的步态角

步骤11、定时器达到t2时刻时，主控制器控制步态电机执行步态角和

步骤12、主控制器利用式(15)计算t3时刻，t3时刻六足履式无人地面车辆的步态如图5所示：

式(15)中，l表示t0时刻中足履位于下方的钝角顶点a1与后足履位于下方的钝角顶点a2之间的距离，如图2所示，并由式(16)获得；

l＝b-2k+2icos2φ(16)

式(16)中，k表示t0时刻中足履中心与中足履位于前方的锐角顶点a3之间的水平距离，如图2，并由式(17)获得；

步骤13、主控制器利用式(18)计算t3时刻前足履的步态角中足履的步态角及后足履的步态角保持不变：

步骤14、定时器达到t3时刻时，主控制器控制步态电机执行步态角

步骤15、主控制器按式(19)计算t4时刻，t4时刻六足履式无人地面车辆的步态如图6所示：

步骤16、主控制器利用式(20)计算t4时刻后足履的步态角前足履的步态角及中足履的步态角保持不变：

步骤17、定时器达到t4时刻时，主控制器控制步态电机执行步态角从而完成上台阶动作成，如图7所示，车身与前、中、后足履均到达台阶上面。