1.一种四足机器人避障及领航员跟随方法,其特征在于,包括如下步骤:获取所有基站的坐标信息、标签到任意一个基站的距离以及原始激光雷达三维点云数据;
对原始激光雷达三维点云数据进行预处理;
基于超宽频三边定位算法,根据所有基站的坐标信息以及标签到任意一个基站的距离,得到四足机器人以及领航员位置信息;
所述基于超宽频三边定位算法进行了改进,改进方式为在计算四足机器人以及领航员位置时引入加权补偿因子矩阵;
引入加权补偿因子矩阵,包括如下步骤:
根据所有基站的坐标信息以及标签到任意一个基站的距离建立方程:通过最小二乘法求解方程,将方程组的前n‑1个方程依次减去最后一个方程并整理成矩阵形式,经过简化得到:AX=b;
其中,
根据最小二乘法原理,可以获得X的最小二乘估计:
考虑实际应用时的外界干扰因素,引入加权补偿因子矩阵W,AX=b进行如下变换:WAX=Wb;
W根据待测标签到每一个基站的距离误差而设计的加权矩阵,此矩阵通常为对称正定矩阵,于是得到标签坐标加权之后的最小二乘解:将经过预处理的激光雷达三维点云数据映射到二维平面栅格地图;
基于二维平面栅格地图和四足机器人以及领航员位置信息建立滑动窗口,基于滑动窗口和自主导航路径规划算法进行路径搜索,得到避障后的最短路径。
2.如权利要求1所述的一种四足机器人避障及领航员跟随方法,其特征在于,对原始激光雷达三维点云数据进行预处理包括点云滤波和地面点去除。
3.如权利要求1所述的一种四足机器人避障及领航员跟随方法,其特征在于,所述二维平面栅格地图包括多个大小相同的栅格,所述栅格表示机器人的工作空间,并且用栅格数组表示环境,所述栅格包括白格区域和黑格区域,所述白格区域代表自由空间,所述黑格区域代表障碍物空间,黑格区域在白格自由空间中所处的位置就是障碍物在四足机器人所处环境中的位置。
4.如权利要求1所述的一种四足机器人避障及领航员跟随方法,其特征在于,所述滑动窗口构建包括:在二维平面栅格地图中,以四足机器人当前位置为原点建立二维坐标平面,滑动窗口的区域占据二维平面坐标的四个象限。
5.如权利要求1所述的一种四足机器人避障及领航员跟随方法,其特征在于,基于滑动窗口和自主导航路径规划算法进行路径搜索包括:将四足机器人的位置设置为路径规划的起点,领航员的位置设置为目标点;当目标点在滑动窗口内移动时,基于自主导航路径规划算法进行路径搜索的同时采用斜向寻路方法,得到最优路径。
6.如权利要求1所述的一种四足机器人避障及领航员跟随方法,其特征在于,所述自主导航路径规划算法为通过代价函数包含的启发信息锁定目标方向搜索最短路径。
7.如权利要求1所述的一种四足机器人避障及领航员跟随方法,其特征在于,在搜索过程中,采用两个状态列表:open列表和close列表用来存放相关的节点信息,所述open列表存储已生成但尚未访问的节点的信息,而close列表存储已访问的节点信息。
8.如权利要求7所述的一种四足机器人避障及领航员跟随方法,其特征在于,找寻节点时,通过在当前位置周围的多个方向上遍历子节点来选择节点。
9.一种四足机器人避障及领航员跟随系统,用于实现如权利要求1‑8任一项权利要求所述的四足机器人避障及领航员跟随方法,包括处理器模块,以及与所述处理器模块连接的环境感知模块,所述环境感知模块包括超宽频模块以及3D激光雷达模块,所述处理器模包括:数据获取模块,被配置为:获取所有基站的坐标信息、标签到任意一个基站的距离以及原始激光雷达三维点云数据;
数据预处理模块,被配置为:对原始激光雷达三维点云数据进行预处理;
四足机器人以及领航员位置信息计算模块,被配置为:引入加权补偿因子矩阵,根据所有基站的坐标信息以及标签到任意一个基站的距离,得到四足机器人以及领航员位置信息;
路径搜索模块,其被配置为:将经过预处理的激光雷达三维点云数据映射到二维平面栅格地图;基于二维平面栅格地图和四足机器人以及领航员位置信息建立滑动窗口,基于自主导航路径规划算法进行路径搜索,得到避障后的最短路径。