一种四足机器人避障及领航员跟随方法及系统与流程

1.本发明属于人工智能和机器人技术领域，尤其涉及一种四足机器人避障及领航员跟随方法及系统。


背景技术：

2.本部分的陈述仅仅是提供了与本公开相关的背景技术信息，不必然构成在先技术。
3.目前，众所周知，地球上的陆地表面积约为1.49亿平方千米，一半以上的陆地表面是崎岖不平的、非结构化环境，而四足哺乳动物可以在类似的环境中高效运动，具有与动态非结构环境的强适应性和运动灵活性。因此，以其为仿生对象，像bigdog、hyq和minicheetah等一系列具有四足动物运动特性的四足机器人涌现出来。在该类机器人的研究中，机器人要能够与人共融，实现自然交互，也能够适应复杂的环境。因此，研究四足机器人的环境适应能力以及人机交互能力成为了重点。其中，解决四足机器人在复杂环境中的避障和人员跟踪问题是当前的一个关键技术难题。
4.在环境感知避障方面，2010年发表在《ieeeinternationalconferenceonroboticsandautomation》的《autonomousnavigationforbigdog》中的bigdog四足机器人很早就实现了在复杂环境下的避障自主行走，将平面激光扫描仪和双目相机获取的障碍物信息转换到2d网格图上，引入路径规划避障算法实现了自主避障。2012年同样发表在《ieeeinternationalconferenceonmechatronicsandautomation》的《obstaclecrossingwithstereovisionforaquadrupedrobot》中的littledog四足机器人通过立体视觉进行感知环境信息，实现了在包括不规则大型障碍物崎岖环境下的行走。除此之外，2013年发表在《ieee/rsjinternationalconferenceonintelligentrobotsandsystems》的《highfidelityday/nightstereomappingwithvegetationandnegativeobstacledetectionforvision
‑
in
‑
the
‑
loopwalking》中的ls3四足机器人搭载了lms111激光扫描仪、二自由度云台、近红外光源与全向近红外相机、bumblebee双目相机等感知工具可以在没有人干预的情况下穿越复杂环境前往指定的地点，实现自主行进。
5.在人员识别跟踪方面，比较常用的方法是识别人体像腿部、人脸、躯干等特定部位，也有学者通过图像识别进行特征提取的方法。然而，目前在四足机器上这一方面的研究较少，但也有学者取得了一些成果。2015年发表在《ieee/rsjinternationalconferenceonintelligentrobotsandsystems》的《leadertrackingforawalkinglogisticsrobot》提出了一种标记跟踪系统，该系统使用近红外摄像机、反光标记和激光雷达，允许特定用户指定自己为四足机器人的领航员，并引导机器人沿着所需的路径前进。2018年发表在《ieeeinternationalconferenceoninformationandautomation》的《leaderrecognitionandtrackingforquadrupedrobots》中利用反射标记区分四足机器人的领航者，并且采用虚拟扩展障碍网格的算法进行路径规划，实现了避障跟踪。
近些年，随着图像识别技 术的迅速发展，有学者基于立体视觉、利用yolov3神经网络进行目标人员 的检测并跟踪，像2019年发表在《chinese control and decisionconference》的《real
‑
time target detection and tracking system basedon stereo camera for quadruped robots》。
6.以上所述的方法中，通过人体特定识别以及通过神经网络目标检测的 方法，虽然可以进行人员跟踪，但在周围人员众多的情况下，四足机器人 可能会丢失先前跟踪的人员，虽然通过对跟踪目标身上张贴特定的反光标 记可以指定跟踪目标，但是张贴标记的材料很容易受到光照环境的影响而 丢失，在多目标跟踪的情况下四足机器人的运动会出现大的波动，跟踪效 果较差。


技术实现要素：

7.为克服上述现有技术的不足，本发明提供了一种四足机器人避障及领 航员跟随方法及系统。通过超宽频定位系统，对手持特定标签的目标人员 进行定位，在此基础上，进一步采用3d激光雷达进行障碍物的检测，结合 定位信息以及障碍物信息进行2d栅格地图的构建，引入路径规划算法，最 终实现了四足机器人的避障和跟踪。
8.为实现上述目的，本发明的一个或多个实施例提供了如下技术方案：
9.一种四足机器人避障及领航员跟随方法，包括如下步骤：
10.获取所有基站的坐标信息、标签到任意一个基站的距离以及原始激光 雷达三维点云数据；
11.对原始激光雷达三维点云数据进行预处理；
12.基于改进的超宽频三边定位算法，根据所有基站的坐标信息以及标签 到任意一个基站的距离，得到四足机器人以及领航员位置信息；
13.将经过预处理的激光雷达三维点云数据映射到二维平面栅格地图；
14.基于二维平面栅格地图和四足机器人以及领航员位置信息建立滑动窗 口，基于滑动窗口和自主导航路径规划算法进行路径搜索，得到避障后的 最短路径。
15.进一步地，对原始激光雷达三维点云数据进行预处理包括点云滤波和 地面点去除。
16.进一步地，所述基于改进的超宽频三边定位算法的改进方式为在计算 四足机器人以及领航员位置时引入加权补偿因子矩阵。
17.进一步地，所述二维平面栅格地图包括多个大小相同的栅格，所述栅 格表示机器人的工作空间，并且用栅格数组表示环境，所述栅格包括白格 区域和黑格区域，所述白格区域代表自由空间，所述黑格代表障碍物空间， 黑格区域在白格自由空间中所处的位置就是障碍物在四足机器人所处环境 中的位置。
18.进一步地，所述滑动窗口构建包括：
19.在二维平面栅格地图中，以四足机器人当前位置为原点建立二维坐标 平面，滑动窗口的区域占据二维平面坐标的四个象限。
20.进一步地，基于滑动窗口和自主导航路径规划算法进行路径搜索包括： 将四足机器人的位置设置为路径规划的起点，领航员的位置设置为目标点； 当目标点在滑动窗口内移动时，基于自主导航路径规划算法进行路径搜索 的同时采用斜向寻路方法，得到最优路
径。
21.进一步地，所述自主导航算法为通过代价函数包含的启发信息锁定目 标方向搜索最短路径。
22.进一步地，在搜索过程中，采用两个状态列表：open列表和close列表 用来存放相关的节点信息，所述open列表存储已生成但尚未访问的节点的 信息，而close列表存储已访问的节点信息。
23.进一步地，找寻节点时，通过在当前位置周围的多个方向上遍历子节 点来选择节点。
24.一个或多个实施例提供了一种四足机器人避障及领航员跟随系统，包 括处理器模块，以及与所述处理器模块连接的环境感知模块，所述环境感 知模块包括超宽频模块以及3d激光雷达模块，所述处理器模包括：
25.数据获取模块，被配置为：获取所有基站的坐标信息、标签到任意一 个基站的距离以及原始激光雷达三维点云数据；
26.数据预处理模块，被配置为：对原始激光雷达三维点云数据进行预处 理；
27.四足机器人以及领航员位置信息计算模块，被配置为：引入加权补偿 因子矩阵，根据所有基站的坐标信息以及标签到任意一个基站的距离，得 到四足机器人以及领航员位置信息；
28.路径搜索模块，其被配置为：将经过预处理的激光雷达三维点云数据 映射到二维平面栅格地图；基于二维平面栅格地图和四足机器人以及领航 员位置信息建立滑动窗口，基于自主导航路径规划算法进行路径搜索，得 到避障后的最短路径。
29.以上一个或多个技术方案存在以下有益效果：
30.(1)本发明提出了一种四足机器人避障及领航员跟随方法，通过搭建 三个基站的uwb定位系统，四足机器人和领航员身上携带uwb标签，利用 纳秒级的非正弦波窄脉冲传输来实现基站和标签之间的无线载波通信，获 取基站与标签之间的距离信息，改进提出了uwb的三边定位算法完成标签 坐标的解算，确定领航员位置信息以及四足机器人位置信息，该定位方法 可使定位精度缩小到0.1m左右。
31.(2)本发明提出了一种四足机器人避障及领航员跟随方法，由于受到 外界环境等不可抗拒的干扰因素，不能保证每一个标签到基站的测量距离 精度的一致性，这种测距精度的差异难免会对最终的定位造成影响，提出 了一种改进的三边定位算法，提高定位精度以及定位的可靠性。
32.(3)本发明更直观地表示环境信息以及更方便的加入路径规划避障算 法，将处理好的激光雷达点云映射到二维平面构建路径规划的栅格地图， 将激光雷达三维点云投影到二维平面、在二维空间中采用栅格法来构建路 径规划的参考地图，这样参考地图简单、实用、操作方便。
33.(4)本发明提出了一种基于滑动窗口的路径规划算法，利用构造的启 发式搜索函数，每一次搜索都能找到可行的最短路径，且具有较高的搜索 效率，解决了传统的寻路算法是一种静态路径规划算法，路径规划后遇到 新的移动障碍物，导致机器人反应速度慢，无法及时调整路线。
附图说明
34.构成本发明的一部分的说明书附图用来提供对本发明的进一步理解， 本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不 当限定。
35.图1为本发明实施例一中四足机器人避障及领航员跟随方法整体流程 图；
36.图2为本发明实施例一中uwb测距算法原理图；
37.图3为本发明实施例一中三边定位法原理图。
38.图4为本发明实施例一中基于自主导航路径规划算法流程图；
39.图5为本发明实施例二中四足机器人避障及领航员跟随整体框架图。
具体实施方式
40.应该指出，以下详细说明都是示例性的，旨在对本发明提供进一步的 说明。除非另有指明，本文使用的所有技术和科学术语具有与本发明所属 技术领域的普通技术人员通常理解的相同含义。
41.需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非 意图限制根据本发明的示例性实施方式。如在这里所使用的，除非上下文 另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的 是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、 步骤、操作、器件、组件和/或它们的组合。
42.在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组 合。
43.实施例一
44.如图1所示，本实施例公开了一种四足机器人避障及领航员跟随方法， 包括如下步骤：
45.s1：获取所有基站的坐标信息以及标签到任意一个基站的距离；
46.本发明通过搭建三个基站的uwb定位系统，所述四足机器人和领航员 身上均携带uwb标签，利用纳秒级的非正弦波窄脉冲传输来实现基站和标 签之间的无线载波通信，获取基站与标签之间的距离信息。
47.所有基站坐标为(x1,y1),(x2,y2),
…
,(x
n
,y
n
),所有标签(x,y)到任意一个基 站的距离分别为d1,d2,...d
n
。
48.所述标签到任意一个基站的距离的计算方法为：
49.基于双边飞行时间双向的uwb测距方法，测量任一基站和标签通信两 端各自时间戳上的两组时间差。
50.如图2所示，作为具体的实施例，例如，假设要实现基站a和标签b 之间的通信测距。
51.基站a在本身的时间戳上打开tx串口通道申请发送信号，向标签b发 送信号后记录下此时的时间戳t
as1
，标签b在自己的时间戳上开启接收信号 的串口通道rx并且接收来自于基站a的信号，接收到信号的同时立马记录 下此时的时间戳t
br1
；
52.经过一段时间后，标签b申请回应已经接受到来自基站a的信号，通 过打开发送串口通道tx向基站a发出回应信号，发送的同时记录回应时的 时间戳t
bs
，基站a打开接收串口驱动rx接收来自标签b的响应信号，同时 记录下此时的时间戳t
ar
；基站a在经过一定时间的延迟处理后，再次通过 已经开启的串口tx通道向标签b发送终极信号，同时记录下发送终
极信号 的时刻t
as2
；标签b再次接收到来自于基站a的终极信号后并记录时间戳 t
br2
。
53.于是，可以得到基站a和标签b的四组时间差为
54.t
round1
＝t
ar
‑
t
as1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
55.t
round2
＝t
br2
‑
t
bs
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)
56.t
reply1
＝t
bs
‑
t
br1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
57.t
reply2
＝t
as2
‑
t
ar
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)
58.因此，实现基站a和标签b通信的无线信号的飞行时间t
prop
为:
[0059][0060]
于是，可以得到任意两者之间的通信距离
[0061]
d
i
＝c
×
t
prop
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
[0062]
式中c为光速。
[0063]
由于传统的基于单边飞行时间双向测距方法要求两个通信测距的设备 的时间戳必须同步，而且在实际应用中的测距精度不是很高，存在偶然波 动，因此本发明采用了双边飞行时间双向测距方法，此方法无需通信双方 的时间同步，同时记录了通信两端各自时间戳上的两组时间差，极大地提 高了测距精度。
[0064]
s2：引入加权补偿因子矩阵，根据所有基站的坐标信息以及标签到任 意一个基站的距离，得到四足机器人以及领航员位置信息。
[0065]
如图3所示，本实施例提出了改进的uwb的三边定位算法完成标签坐 标的解算，确定领航员位置信息以及四足机器人位置信息，该定位方法可 使定位精度缩小到0.1m左右。
[0066]
包括如下步骤：
[0067]
根据所有基站的坐标信息以及标签到任意一个基站的距离建立方程：
[0068][0069]
通过最小二乘法求解方程(1)，将方程组的将方程组的前n
‑
1个方程 依次减去最后一个方程并整理成矩阵形式：
[0070][0071]
经过简化得到：
[0072]
ax＝b
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(9)
[0073]
其中，
[0074]
[0075][0076][0077]
根据最小二乘法原理，可以获得x的最小二乘估计
[0078][0079]
考虑实际应用时的外界干扰因素，引入加权补偿因子矩阵w，式子(9) 进行如下变换：
[0080]
wax＝wb
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(14)
[0081]
w根据待测标签到每一个基站的距离误差而设计的加权矩阵，此矩阵 通常为对称正定矩阵。于是得到标签坐标加权之后的最小二乘解：
[0082][0083]
传统的三边定位算法是根据三个基站的位置坐标以及标签距离三个基 站之间的距离来获得标签的位置，这种算法直观简便，对于由三个基站搭 建的场地模型，三边定位算法是比较常用且容易实现的一种定位算法。然 而在实际应用中，由于受到外界环境等不可抗拒的干扰因素，不能保证每 一个标签到基站的测量距离精度的一致性，这种测距精度的差异难免会对 最终的定位造成影响，因此，为了进一步的提高定位精度以及定位的可靠 性，在原有三边定位算法的基础上，本发明提出了一种改进的三边定位算 法。
[0084]
s3：获取完整的原始激光雷达三维点云数据，对原始点云数据进行预 处理；
[0085]
为了让四足机器人尽可能地检测出不同尺寸的障碍物，同时，考虑到 在实际应用中，由于激光雷达搭载于四足机器人上方，会出现雷达平面无 障碍物，但雷达以下平面有障碍物的情况，基于这些问题，本发明采用16 线3d激光雷达，有影响的障碍物都会被探测到，提高了障碍物探测的鲁棒 性。16线3d激光雷达虽然可以获取大量完整的原始点云信息，但也会由于 外部的影响，点云数据中不可避免的出现一些无效点，因此首先对原始点 云进行预处理。
[0086]
具体的，所述原始点云数据进行预处理过程包括点云滤波和地面点去 除。
[0087]
(1)点云滤波为通过统计滤波算法对三维点云进行大尺度噪声去除。
[0088]
所述统计滤波算法的主要思想是假设三维点云中的每个查询点与其邻 域点的平均距离满足高斯分布，通过其均值和方差确定一个距离阈值，在 距离阈值范围之外的点就判定为离群点并去除。
[0089]
具体的步骤包括：
[0090]
假设三维点云中某个点r邻域内有m个邻域点，r到每个邻域点的距离 为d
j
，于是，可以计算出点r到其m个邻域点的平均距离d
ave
为
[0091]
[0092]
然后，对平均距离进行统计分析，计算出均值μ和标准差σ，根据两者 设置距离阈值d
thr
[0093]
d
thr
＝μ
±
σ
·
α
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(17)
[0094]
其中，α为阈值系数。
[0095]
最后，通过判断点r的m个近邻邻域点的平均距离d
ave
是否大于距离阈 值d
thr
。若大于距离阈值d
thr
，则点r为离群点并去除。
[0096]
激光点云地图存储的是传感器对环境的原始扫描点云，优点是保留信 息完整，缺点是在获取点云数据时，由于设备精度，操作者经验环境因素 带来的影响，以及电磁波的衍射特性，被测物体表面性质变化和数据拼接 配准操作过程的影响，点云数据中不可避免的出现一些离散点、孤立点， 不能直接用于导航避障，因此，进行点云滤波预处理。
[0097]
本实施例通过统计滤波算法对三维点云进行大尺度噪声去除，使不规 则的点云数据密度变得平滑，去除了因为遮挡等问题造成的离群点以及不 必要的噪声数据。
[0098]
(2)地面点去除
[0099]
所述地面点去除包括对点云进行高度裁剪，剪裁掉过高的点云信息， 采用ransac(random sample consensus)随机采样一致性算法，找出代表地 面的平面。
[0100]
该算法的核心思想为：整个点云数据的分布可以通过数学模型来描述， 从原始点云数据集中随机选取三个点，根据这三个点确定一个平面模型。 利用这个数学模型去估算其他的点，适应该模型的点称为局内点，不可以 适应该模型的点便是局外点；重复迭代多次，每次产生的模型要么因为局 内点个数太少而被舍弃，要么就比现存模型更好而被选用，最终确定出一 个最优的平面数学模型。
[0101]
具体的步骤如下：
[0102]
空间平面的法线式平面方程为
[0103]
ax by cz＝l
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(18)
[0104]
上式中，(a,b,c)为平面的单位法向量，满足a2 b2 c2＝1,l表示平面到 原点之间的距离。
[0105]
从点云数据集中随机取三个点q1(x1,y1,z1),q2(x2,y2,z2),q3(x3,y3,z3)，代入式 (18)中得到式(19)，求出a,b,c三个参数，确定出平面方程s。
[0106][0107]
通过该模型去判断点云数据集中的其他点是否为局内点，设定点到平 面的距离作为判断准则，见式(20)
[0108]
l
i
＝|ax
i
by
i
cz
i
‑
l|
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(20)
[0109]
具体的，需要根据实际点云数据设定合适的距离阈值θ来近似拟合平 面，在阈值θ范围之内的点就符合该模型，便是局内点，超过距离阈值θ的 点就是局外点，统计该模型下局内点的数量；
[0110]
上面的过程需要迭代多次，选择局内点数量最大时的那组点,将最大数 量的局内点进行拟合得到最终的平面模型，因此，需要确定合适的迭代次 数k。用p表示随机抽到局内点的概率，用c表示置信度，这个参数表示 ransac算法在运行后提供有用结果的期望概
率。n表示计算模型参数需要 选取的数据个数，k表示迭代次数，p
n
表示随机抽取的n个点都是局内点的 概率，1
‑
p
n
表示至少存在一个局外点的概率，(1
‑
p
n
)
k
表示迭代k次都至少 存在一个局外点的概率，当k足够大时，这个概率会变得很小，意味着模型 可信度越高，那么迭代k次至少可以获得一组正确模型参数的概率为 1
‑
(1
‑
p
n
)
k
,这个概率等于置信度参数c，有
[0111]
c＝1
‑
(1
‑
p
n
)
k
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(21)
[0112]
对上式取对数，则反推k的值为
[0113][0114]
k就是需要的迭代终止条件，当实际迭代次数大于k时，退出循环。
[0115]
环境感知通常只对路面上的障碍物感兴趣，地面点云易对障碍物聚类 产生影响，所以在做障碍物聚类之前通常将地面点云和非地面点云进行分 离，将地面和非地面分割出来。首先需要先对点云进行高度裁剪，将过高 的区域忽略不计。本发明机器人平台激光雷达安装高度约为0.5米，剪裁 掉过高的点云信息，从而分割出地面点云以减少激光雷达对障碍物聚类的 影响。然后，采用ransac(random sample consensus)随机采样一致性算法， 找出代表地面的平面。
[0116]
s4：将经过预处理的激光雷达点云数据映射到二维平面栅格地图；
[0117]
所述二维平面栅格地图中，用栅格表示环境是用大小相同的栅格来划 分机器人工作空间，并且用栅格数组表示环境。障碍物存在于栅格所表示 的环境中，并占用一定数目的栅格。用黑格代表障碍物空间，用白格代表 自由空间，则黑格区域在白格自由空间中所处的位置就是障碍物在四足机 器人所处环境中的位置。
[0118]
为了更直观地表示环境信息以及更方便的加入路径规划避障算法，将 处理好的激光雷达点云映射到二维平面构建路径规划的栅格地图。
[0119]
不考虑三维空间中的物体高度问题，并且假设障碍物的高度是在激光 雷达能够检测到的高度范围内，将激光雷达三维点云投影到二维平面、在 二维空间中采用栅格法来构建路径规划的参考地图，这样参考地图简单、 实用、操作方便。因此，结合障碍物点云信息和定位信息进行了点云二维 栅格化的操作。
[0120]
在实际中，为了更加精确地描述周围环境信息，将划分的每个网格大 小代表的精度是0.1m。考虑到在实际应用情况下的处理器的运算速度以及 路径规划算法遍历网格的效率，所转换的栅格地图大小定义为200*200。
[0121]
s5：基于二维平面栅格地图和四足机器人以及领航员位置信息建立滑 动窗口，基于滑动窗口和自主导航路径规划算法进行路径搜索，得到避障 后的最短路径。
[0122]
对于四足机器人来说，它的位置相当于路径规划的起点，领航员的位 置相当于路径规划的目标点，由于四机器人是跟随领航员行走的，探知的 环境信息会随着四足机器人位置的变化而实时更新，这导致路径规划的起 点与目标点不再是固定的地理坐标，然而传统的寻路算法是一种静态路径 规划算法，路径规划后遇到新的移动障碍物，导致机器人反应速度慢，无 法及时调整路线，因此本发明使用了基于滑动窗口的路径规划算法。
[0123]
所述滑动窗口的构建过程具体包括如下步骤：
[0124]
(1)将四足机器人的位置设置为路径规划的起点，领航员的位置设置 为路径规划的目标点；
[0125]
(2)建立滑动窗口，所述滑动窗口的建立过程为：在栅格地图中以四 足机器人当前位置为原点建立二维坐标平面，滑动窗口的区域占据二维平 面坐标的四个象限。
[0126]
当目标点在滑动窗口移动时，以四足机器人当前位置为起点，以可移 动目标点当前位置为终点，利用自主导航算法进行计算，得到最优路径， 同时采用8方向的斜向寻路方法，大大提高了路径搜索的效率。
[0127]
所述通过构造启发式搜索函数，搜索得到避障后的最短路径，具体包 括：
[0128]
所述启发式搜索函数的构建过程包括如下步骤：
[0129]
所述代价函数为：
[0130]
f(n)＝g(n) h(n)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(23)
[0131]
式中：f(n)表示地图中从起始位置到目标节点总的代价函数，g(n)用于 表示机器人从初始状态的节点到当前n节点之间的实际代价值的大小，h(n) 包含启发信息，表示从当前n节点至最终目标点对应节点的最小估计代价值 的大小。
[0132]
其中，g(n)是一个定值，所以h(n)的求解至关重要。
[0133]
假设启发函数h
*
是节点n到终点的最优消耗值。
[0134]
h(n)和h
*
(n)的关系可分为以下三种情况：
[0135]
(1)h(n)>h
*
(n)，可能会在较短时间内找到一条通往终点的路径，但该 路径可能不是最优解。
[0136]
(2)h(n)＝h
*
(n)，即每个扩展点都是最优路径上的点，处理结果一定会 有最优解。
[0137]
(3)h(n)<h
*
(n)，处理结果一定能找到一条最优路径，但可能会经过部 分无效点。
[0138]
在特殊情况下，当h(n)＝0，f(n)＝g(n)时，自主导航算法转变为广度优 先搜索算法(breadth first search，bfs)，导致该算法失去启发性。
[0139]
只有在h(n)≤h
*
(n)的情况下，算法才具有完成和实现最优的能力。实际 应用中，如果想要准确的获得最优路径，就需要获得更多的启发信息量， 如：待选节点与终点的关系、待选节点的权重等。但是，太多的约束会增 加计算量，计算最优路径的时间会更长。
[0140]
优选地，所述求解合适h(n)类型时，采用频率最高的启发式函数为曼哈 顿距离。它考虑了从一个地点到另一个地点的最小成本。曼哈顿距离公式 如下:
[0141]
h(n)＝|x
g
‑
x1| |y
g
‑
y1|
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(24)
[0142]
其中，(x1,y1)对应于路径规划中目前机器人坐标的位置，(x
g
,y
g
)代表在 路径规划中设置的目标点的坐标位置。
[0143]
所述自主导航算法的流程图如图4所示。
[0144]
(1)将机器人起点放入open列表中；
[0145]
(2)寻找机器人起点周围可到达的节点，跳过close列表中的点，将该 点作为周围节点的父节点；
[0146]
(3)开启列表删除该机器人起点，并放到close列表中；
[0147]
(4)计算open列表中的值；
[0148]
(5)判断代价函数是否最小，若是，则寻找列表成功，把该点从open 列表加入close列表中，否则将该点加入close列表中；
[0149]
(6)判断该点是否为目标点，若是则得到最短路径，否则重复上述步 骤(1)
‑
(5)。
[0150]
在搜索过程中，主要使用两个状态列表：open列表和close列表用来存 放相关的节点信息。
[0151]
其中open列表主要存储已生成但尚未访问的节点的信息，而close列表 主要存储已访问的节点信息。
[0152]
搜索开始时，close列表为空，只有一个节点即开始节点在open列表中， 开始反复搜索，获得期望得到的节点。
[0153]
如果此节点没有到达目标点，则继续搜索该节点的相邻节点，然后把 找寻到的新节点放入到open列表中，若找寻到的新节点在open列表中，则比 较其代价值，若代价值低，则更新open列表的信息，直到达到目标位置。 如果open列表在到达目标点之前为空，则代表不存在路径。找寻节点时， 通过在当前位置周围的所有方向上遍历子节点来选择节点。
[0154]
所述子节点选择过程包括：
[0155]
(1)如果子节点为障碍物，则忽略这个节点。
[0156]
(2)如果节点在open列表中，则需要判断该节点。如果选择的当前子 节点的路径代价小于覆盖父节点的路径代价，则将当前节点设置为子节点 的父节点。然后修改子节点的f(n)和h(n)值，否则将忽略子节点。
[0157]
(3)如果不满足上述情况，则找到一个新生成的节点，并将其插入到 open列表中。
[0158]
实施例二
[0159]
本实施例公开了一种四足机器人避障及领航员跟随系统，如图5所示 包括：四足机器人平台、环境感知模块以及处理器模块，所述四足机器人 平台包括四条腿和腿部连接的机器人平台，所述每条腿由三个48牛米的无 刷直流电机组成，分为转角180度的横滚髋关节、转角180度的俯仰髋关 节以及转角150度小腿俯仰关节，腿部采用铝合金连杆制成。
[0160]
所述环境感知模块包括超宽频(ultra wideband，uwb)模块和3d激 光雷达模块，所述3d激光雷达模块设置于超宽频模块设置于机器人平台，
[0161]
所述机器人下位机控制器采用up board进行离线运动解算，使用can 总线实现与电机通信，控制机器人运动。
[0162]
所有设备都通过网络与网络交换机连接，构成机器人ros控制网络。
[0163]
优选地，本实施3d激光雷达模块采用混合固态激光雷达模式，集成了 16个激光收发组件，测量距离高达150米，测量精度4cm以内，每秒钟出 点数高达300000点，水平测角360
°
，垂直测角
‑
15
°
～15
°
。
[0164]
在具体实施中，要求四足机器人躲避前方障碍并跟踪前方目标人员， 因此可以忽略四足机器人后面的障碍物信息，将水平视场角设置为180
°
， 完全能达到预期目标。
[0165]
所述超宽频模块采用link track型号，这是一款基于uwb技术的多功 能系统，支持局部定位功能，支持配置为标签、基站等多种角色。局部定 位是支持定位、导航、授时与通信一体化功能的实时定位模式，分为标签、 基站、控制台三种角色。标签实时测量并进行坐标解算，输出自身测距、 坐标等信息，基站与控制台实时输出所有标签的定位信息。
[0166]
包括：
[0167]
数据获取模块，被配置为：获取所有基站的坐标信息以及标签到任意 一个基站的距离；
[0168]
数据预处理模块，被配置为：获取原始激光雷达三维点云数据，对原 始激光雷达
三维点云数据进行预处理；
[0169]
四足机器人以及领航员位置信息计算模块，被配置为：引入加权补偿 因子矩阵，根据所有基站的坐标信息以及标签到任意一个基站的距离，得 到四足机器人以及领航员位置信息；
[0170]
路径搜索模块，其被配置为：将经过预处理的激光雷达三维点云数据 映射到二维平面栅格地图；
[0171]
基于二维平面栅格地图和四足机器人以及领航员位置信息建立滑动窗 口，基于自主导航路径规划算法进行路径搜索，得到避障后的最短路径。
[0172]
本领域技术人员应该明白，上述本发明的各模块或各步骤可以用通用 的计算机装置来实现，可选地，它们可以用计算装置可执行的程序代码来 实现，从而，可以将它们存储在存储装置中由计算装置来执行，或者将它 们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成 单个集成电路模块来实现。本发明不限制于任何特定的硬件和软件的结合。
[0173]
上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本 发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案 的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或 变形仍在本发明的保护范围以内。