一种仿蛇机器人自适应路径跟踪方法与流程

本发明涉及机器人研究与工程领域，具体是指一种复杂环境下仿蛇机器人路径规划与自适应跟踪方法。

背景技术：

随着科技的进步，机器人被广泛地应用于各个领域来替代人类完成繁杂的任务，军事侦察行动、科学探索行动、工业检修及灾后救援都不乏机器人的身影。与生物蛇的多冗余物理结构特性相似，仿蛇机器人具有多自由度和灵活的运动方式，能够实现鲁棒和稳定的多种运动方式，如蜿蜒运动、直线运动、伸缩运动及侧向运动，能够在复杂环境中高效运动，受到研究人员的关注。

仿蛇机器人依靠关节的节律摆动，与地面之间产生各向异性摩擦力，推动机体向前运动。由于其特殊的运动机理，导致运动的过程会产生侧向偏移，无法始终保持直线运动。当仿蛇机器人处于平坦开阔的地面环境中，这种运动机理和侧向偏移的运动特点不会影响任务执行。然而，当机器人处于障碍较多、可通行路径狭窄的地形时，按照原有的步态控制方法，机器人不能精确的沿着道路中间移动，容易碰撞到道路两侧的障碍物。目前仿蛇机器人能够在简单的地面环境中较好的运动，但是在复杂的地形中，仿蛇机器人的多关节、多自由度为完全避障的路径规划以及精确的路径跟踪带来了艰巨的困难。路径规划是机器人研究领域内的重要分支，其目的是在一定约束条件下使机器人在多障碍物环境中生成一条通向目标点的无障碍物路径。研究人员提出多种路径规划算法，如人工势场法、遗传算法及蚁群算法等，然而由于仿蛇机器人运动的姿态多变性，导致这些算法不能很好的直接应用于仿蛇机器人。路径跟踪同样是机器人研究领域内的重要环节，其目的是控制机器人从某处出发，沿着给定的路径到达目标点。由于其关节摆动导致运动过程中产生方向偏移，使其仿蛇机器人难以精准的跟踪给定参考路径。路径规划与跟踪是实现仿蛇机器人自主导航的关键技术，研究适用于仿蛇机器人的效率高、实时性好的路径规划方法和准确度高、稳定性好的路径跟踪算法对于提升仿蛇机器人自主化和智能化具有重要意义。

针对复杂环境下的地形特点，路径规划方法应该具备高效性和实时性，且适用于仿蛇机器人多关节、多自由度机构特点和节律摆动运动机理。路径跟踪策略也应该具备不过分偏离航线的准确性及不产生剧烈摆动行为的稳定性。然而目前尚未有充分满足以上要求的仿蛇机器人路径规划和跟踪方法。

技术实现要素：

针对上述问题，本发明提供一种仿蛇机器人在复杂环境下的路径规划与自适应路径跟踪方法，以解决目前已有方法不适用于仿蛇机器人在复杂环境下运动的难题，即规划路径实用性差、路径跟踪不精准等。具体措施是将改进的rrt算法应用于仿蛇机器人的路径规划并设计新的自适应路径跟踪控制策略。首先，确定路径规划目标点，根据仿蛇机器人姿态框架对障碍物进行膨胀化处理，建立机器人和障碍物之间的碰撞约束，利用路径规划算法生成可行路径点。其次，针对仿蛇机器人无法大幅变向的运动特点，对路径点进行筛选、平滑处理。在路径跟踪过程中，将仿蛇机器人视为一个整体并以可变矩形框架表示，以可变矩形与膨胀化后的障碍物之间的碰撞关系调整仿蛇机器人的步态参数。仿蛇机器人的路径规划与自适应跟踪算法，通过路径规划及平滑克服了现有算法实用性差、不稳定的缺陷，通过设计相应的自适应路径跟踪控制器能够实现仿蛇机器人的自主导航，极大提升仿蛇机器人的环境下适应能力，使其能够执行灾后搜救、军事侦察、工业检修及等任务。

本发明除了提出路径规划以外，还设计了相应的自适应路径跟踪控制方法。自适应路径跟踪控制器中的前向距离影响着机器人收敛到期望路径的速率，因此本发明设计了该参数的自适应调整函数，提升路径跟踪的精确性。

本发明仿蛇机器人路径规划与自适应跟踪方法适应于仿蛇机器人在复杂环境下执行任务，当仿蛇机器人进入新的工作环境中，将通过以下步骤实现路径规划与自适应跟踪：

如图1，仿蛇机器人进入新的任务环境后，需要进行以下几步：①利用传感系统获取环境信息(可行范围、障碍物位置、障碍物大小等)，明确任务需求(起始位置、目标位置、运动速度等)，并根据获取的环境信息生成栅格地图；②生成仿蛇机器人整体框架，对障碍物膨胀进行膨胀化处理，建立机器人矩形框架和膨胀化后的障碍物之间的碰撞约束，运用rrt路径规划算法获取可行路径点，根据机器人整体姿态筛选、平滑路径点，得到合适路径；③考虑仿蛇机器人整体框架与障碍物的位置信息,调整路径跟踪过程中仿蛇机器人的步态控制方程中的参数α；④根据仿蛇机器人路径跟踪过程中位置、方向的偏离程度，自适应计算跟踪控制器中的前向距离δ，并计算步态控制方程中的关节角偏移量⑤根据所得数据计算各个关节的实时转动角度；⑥将角度值传输给对应关节，实现仿蛇机器人按照期望步态运动。

本发明仿蛇机器人路径规划与自适应跟踪，是采用以下技术方案实现的：

所述仿蛇机器人整体框架是根据机器人步态参数生成的将整个机体包含在内的矩形，所述障碍物膨胀化是根据仿蛇机器人整体框架矩形大小对障碍物虚拟外延，避免整体框架与膨胀化障碍物的重合即可实现仿蛇机器人无碰撞运动。

所述跟踪控制器参数为影响仿蛇机器人收敛到期望路径速率的前向距离，前向距离增大将加快收敛速率但降低稳定性，前向距离减小时将降低收敛速率但提升稳定性。

通过上述步骤可行路径的生成和跟踪过程中仿蛇机器人步态参数及路径跟踪参数自适应调整，仿蛇机器人能够在多障碍物复杂环境中生成高适应性路径并能够实时计算仿蛇机器人的步态调整参数，实现仿蛇机器人的路径规划与自适应跟踪。

本发明由于采取以上技术方案，其具有以下优点：

本发明所提出规划前的仿蛇机器人整体矩形框架生成、障碍物膨胀化处理和路径点筛选、平滑方法充分考虑到了仿蛇机器人运动特点，有效地将原有的路径规划算法应用于仿蛇机器人，有效提高了仿蛇机器人对多障碍物复杂环境的适应能力。

本发明所提出的自适应路径跟踪方法能够对仿蛇机器人运动过程中步态做出适应性调整，提升了跟踪已规划路径的精度并降低了跟踪过程中与障碍物发生碰撞的可能性，有效提高了仿蛇机器人对多障碍物复杂环境的适应能力，对于提升仿蛇机器人实用性具有重要意义。

附图说明

图1是本发明的实施流程图；

图2是本发明的仿蛇机器人路径规划与自适应跟踪方法所依托的正交关节仿蛇机器人机构示意图；

图3是本发明的仿蛇机器人路径规划与自适应跟踪方法运用到的仿蛇机器人水平波动步态示意图；

图4是仿蛇机器人整体矩形框架与障碍物膨胀化示意图；

图5是仿蛇机器人进行路径点筛选与平滑处理示意图；

图6是仿蛇机器人路径跟踪示意图；

图6.1是仿蛇机器人路径跟踪过程中整体姿态自适应调整示意图；

图6.2是自适应视觉线路径跟踪方法示意图；

图6.3是仿蛇机器人路径规划与自适应路径跟踪示意图。

具体实施方式

下文结合附图对本发明做进一步说明。

图3为本发明用到的水平波动步态。如图中所示，水平波动是通过控制偏航关节节律摆动实现的节律运动，各偏航关节角度控制函数为：

φi,ref＝αsin(ωt+(i-1)δ)+φ0,i＝1,2...nh(1)

式中，nh为偏航关节个数，α和ω分别为偏航关节摆动的幅值和频率，δ为偏航关节之间相位差，为偏航关节摆动角度偏移值，i为偏航关节的序号数。

参见图4，本发明提供的仿蛇机器人整体框架与障碍物膨胀。其中仿蛇机器人的整体框架为基于仿蛇机器人质心所处位置、仿蛇机器人的步态参数及前进方向生成的能够将完整机体包括的可变矩形,其的相关函数如下：

其中n为连杆数量，l为连杆长度，(xj，yj)为连杆j的质心的全局坐标，(px，py)为仿蛇机器人质心的全局坐标，(an，bn)为不考虑仿蛇机器人前进方向时整体框架第n个顶点的坐标，(un，vn)为仿蛇机器人整体框架的第n个顶点的坐标，为仿蛇机器人方向角，α为仿蛇机器人蜿蜒运动的幅值，tole＞0为一个参数常量，该值影响所构框架对仿蛇机器人多变运动位姿的宽容度。

障碍物膨胀为对原始障碍物的碰撞影响范围进行一定程度的扩张，以此降低仿蛇机器人自身运动特点导致的碰撞可能性，其膨胀程度依据仿蛇机器人运动振幅进行调整，其相关函数如下：

其中，no为环境中障碍物的数目，(xok，yok)为环境中第k个障碍物左下顶点的全局坐标，(xok，yok)为第k个障碍物膨胀后的左下顶点的全局坐标，l，w分别为障碍物的长、宽，α为运动幅值。

参见图5，本实验提供的仿蛇机器人路径点平滑方法是对规划的路径点进行筛选，提高规划路径对于仿蛇机器人的适用性。其具体方法为若下一路径点的前进方向与当前路径点的前进方向偏差较小，则保留，判断删除下一路径导航点后的路径是否会与障碍物发生碰撞，是则执行删除操作。

参见图6.1，本实验提供的仿蛇机器人在路径跟踪过程中将依据仿蛇机器人与障碍物的距离对其步态参数做出自适应调整，在降低与障碍物的碰撞可能性同时尽可能保证前进速度。

图6.2为所提路径跟踪方法中自适应视觉线导引律。如图6.2所示，曲线为已规划出的路径，(px，py)为仿蛇机器人质心位置，(x1，y1)为机器人质心正下方的路径点，(x2，y2)是机器人运动方向与参考路径的交点，通过(px，py)向直线(x1-x2，y1-y2)作垂直线，(x3，y3)为交点，将直线(x1-x2，y1-y2)上与点(x3，y3)相隔δ2的点(x4，y4)作为机器人此刻的目标位置。图中参数计算公式为：

图6.2为所提路径跟踪方法中自适应视觉线导引律。如图6.2所示，仿蛇机器人的路径跟踪将通过控制关节角度偏差值实现，其计算公式为：

式中，为仿蛇机器人整体方向角，为依照视线控制律所定义的参考方向角,计算公式为：

式中，θj为连杆j和全局x轴之间的角度,δd为跟踪路径误差，δ2为前向距离。