一种有人驾驶的足式移动平台人机路径优选及协同方法

本发明属于足式机器人路径规划，具体涉及一种有人驾驶的足式移动平台人机路径优选及协同方法。

背景技术：

1、足式移动机器人因具备稳定性好、负载能力强、控制可靠，地形适应能力强的特点，使其在救灾抢险、野外运输、军事作战、星球探测等复杂场景中广泛应用的潜力。足式移动机器人虽然具有应对各种未知环境的能力，但其机械结构十分复杂，控制难度也相应增大，不断增加的控制要求同时也提高了驾驶员的操纵负担；另一方面足式移动机器人不仅要在相对平坦的地面上行驶，而且还要穿越各种崎岖地形和斜坡障碍等未知复杂环境，使得足式移动机器人的路径跟踪控制很难做到完全“自主”。

2、人机协同路径规划，即驾驶员和足式移动平台共享路径选择权成为一种中间替代方案，这其中不可避免地涉及到驾驶员和足式移动机器人两者之间相互影响，甚至发生冲突的情况；因此，如何实现驾驶员和足式移动机器人路径权限的分配与融合是实现人机协同路径规划必须解决的问题，本发明将纳什博弈理论的思想融入到人机协同路径规划中，在保证完成任务的前提下，既能充分发挥足式移动机器人的自主智能，又能充分发挥驾驶员异常情景处理与经验总结能力，并减轻驾驶员驾驶负担。

技术实现思路

1、本发明为解决现有技术的不足，现提供一种有人驾驶的足式移动平台人机路径优选及协同方法，本发明提供以下技术方案：

2、一种有人驾驶的足式移动平台人机路径优选及协同方法，其特征在于：足式移动平台路径规划模块根据已知全局地图执行内嵌路径规划算法实现足式移动平台路径规划；驾驶员路径规划模块根据反馈地形实现驾驶员路径规划；人机路径融合模块依据足式移动平台与驾驶员收益函数对足式移动平台规划路径与驾驶员规划路径进行动态加权融合；路径评估函数实时评估跟踪路径，对不可通过路径进行局部路径重规划；具体包括以下步骤：

3、步骤1：首先建立可通过代价地图，足式移动平台规划模块依据代价地图利用快速搜索随机树算法搜索路径得到足式移动平台规划路径pr；驾驶员规划模块根据反馈地形得到驾驶员规划路径pd；

4、步骤2：利用人机路径融合模块将足式移动平台规划路径pr与驾驶员规划路径pd进行融合，得到最优跟踪路径p，并更新地图路径信息，足式移动平台依据路径p进行跟踪；

5、步骤3：通过路径评估函数实时评估路径p，遇到环境、平台状态突变导致路径p不可到达目标点时，通过混合rrt-apf算法进行局部路径重规划，得到重规划路径ps并更新重规划路径信息。

6、所述步骤1具体为：

7、将已知地图转换成高程栅格地图，利用坡度角s、相邻单元地形的高度差h、单元地形的宽度和w、地形粗糙度r，分别计算斜坡地形代价costs、垂直障碍地形代价costh、沟壑地形代价costw以及粗糙地形代价costr，建立可通过代价函数eera，完成全局可通过代价地图建立，如式(1)所示：

8、

9、式中，ε表示一个接近但不为零的值，τ为设定的阈值；

10、在标定目标节点后，利用快速搜索随机树算法搜索路径，以足式移动平台与目标点距离、与障碍物距离、路径能量消耗作为评估条件，将评估结果最优的点作为下一节点，如式(2)所示：

11、

12、式中，er表示评估结果，(xi,yi)表示足式移动平台当前坐标，(xg,yg)表示目标点坐标，rr表示以足式移动平台所在圆的半径，robs表示以障碍物所在圆的半径，fi表示关节i的关节力矩，θi表示关节i的转动角度，由先验知识确定权重因子a1、a2、a3；当节点到达目标节点时，即完成足式移动平台规划路径pr；

13、驾驶员利用外置设备在人机交互显示终端内绘制初始规划路径，系统以足式移动平台与目标点距离、与障碍物距离、路径舒适程度作为评估条件，对初始规划路径进行评估，如式(3)所示：

14、

15、式中，ed表示驾驶员路径评估结果，(xi,yi)表示足式移动平台当前坐标，(xg,yg)表示目标点坐标，rr表示以足式移动平台所在圆的半径，robs表示以障碍物所在圆的半径，hi表示驾驶员视野窗口内某一栅格的高程值，表示驾驶员视野窗口内平均高程值，b1、b2、b3表示各自的权重因子；以评估结果最优的初始路径作为驾驶员规划路径pd；

16、随后采用三次均匀b样条插值方法对得到的足式移动平台规划路径pr、驾驶员规划路径pd进行平滑处理。

17、所述步骤2具体为：

18、以足式移动平台和驾驶员作为博弈双方构建人机路径非合作完全信息静态博弈模型，将驾驶员操作复杂度、驾驶员路径平滑性、足式移动平台期望路径的能量消耗和足式移动平台期望路径稳定裕度作为评估条件，建立足式移动平台与驾驶员收益函数j，如式(4)所示：

19、

20、式中，j表示足式移动平台或者驾驶员的收益结果，w1,w2,w3,w4为各评估条件的权重，由加权平均法求得，且w1+w2+w3+w4＝1；

21、构建博弈双方的最优反应函数方程，如式(5)所示，设驾驶员选择路径pd的概率为p，选择路径pr的概率为1-p，足式移动平台选择路径pd的概率为q，选择路径pr的概率为1-q，则有：

22、

23、式中，jdpp表示驾驶员选择pd与足式移动平台选择pd时驾驶员收益，jdpg表示驾驶员选择pd与足式移动平台选择pr时驾驶员收益，jdgp表示驾驶员选择pr与足式移动平台选择pd时驾驶员收益，jdgg表示驾驶员选择pr与足式移动平台选择pr时驾驶员收益，jrpp表示足式移动平台选择pd与驾驶员选择pd时平台收益，jrpg表示足式移动平台选择pd与驾驶员选择pr时平台收益，jrgp表示足式移动平台选择pr与驾驶员选择pd时平台收益，jrgg表示足式移动平台选择pr与驾驶员选择pr时平台收益；

24、通过反应函数方程求得两者选择策略最优概率(p*,q*)，得到两者选择策略最大收益(jdmax，jrmax)，由最大收益结果可以确定人机路径权重因子α如式(6)所示：

25、

26、足式移动平台规划路径pr与驾驶员规划路径pd依据人机路径权重因子α进行动态加权融合，得到最优跟踪路径p，计算如式(7)所示：

27、p＝αpr+(1-α)pd (7)

28、所述步骤3具体为：

29、以环境状态即可通过代价函数eera、平台状态ercs作为评估条件建立路径评估函数edef如式(8)所示：

30、edef＝c1eera+c2ercs (8)

31、式中，ercs＝β1essm+β2ekin+β3ecol，essm为稳定裕度约束，ekin为运动学约束，ecol为无碰撞约束，β1、β2、β3分别为各自的权重因子，c1、c2分别表示环境状态、平台状态权重因子；

32、当路径评估结果确定当前跟踪路径p不能到达目标点，需要依据当前位置节点与目标节点重新规划更加稳定收敛的局部路径，选取足式移动平台当前位置作为局部路径起始节点，初始路径中与受阻区域不接触的最近节点作为局部路径目标节点，使用apf引导rrt搜索树扩展新节点，如式(9)所示：

33、

34、式中，pnew表示扩展的新节点，表示距离向量pnear表示扩展树中距离随机采样节点最近的节点，表示障碍物的排斥力以及目标点的吸引力间的合力，即α、β表示控制rrt部分与apf部分比例的参数，由式(10)确定：

35、

36、式中，r为搜索空间范围半径；robs为障碍物所在圆的半径；当pnew节点到达局部路径目标节点则代表已完成局部路径重规划，得到重规划路径ps，足式移动平台即从当前位置沿此重规划路径继续跟踪，直至到达目标点。

37、本发明具有以下有益效果：

38、所述的一种有人驾驶的足式移动平台人机路径优选及协同方法，通过人机协同规划的方式一方面提升足式移动机器人的自主智能，另一方面在具有平台自主智能的基础上，充分融合驾驶员面对复杂多场景及各种突发异常的处理能力与经验，在降低驾驶员的操作负担基础上，可以使得平台能够在复杂环境下人机协同完成预期任务。