一种基于超声波测量的机器人室内导航方法技术

本发明专利技术提供一种基于超声波测量的机器人室内导航方法，应用于室内走廊上，其包括以下步骤：将两个超声波模块分别固定于机器人左右两侧对称的位置上，且两个超声波模块的法线方向与地面的角度为；设定机器人在室内走廊上的运行轨迹，机器人运行；分别获得机器人与两侧墙壁之间的实际距离，以及走廊宽度；根据两个超声波模块获得的上述数据进行分析计算不同情况下机器人沿轨迹法向位置偏差的变化量；然后计算机器人修正后的偏向角，并根据该偏向角调整机器人的运行轨迹，是的当前运行轨迹与设定运行轨迹重合。本发明专利技术对机器人的航向角信息进行修正，提高了机器人长时间运行的可靠性。

【技术实现步骤摘要】

本专利技术涉及导航控制
，尤其涉及一种基于超声波测量的机器人室内导航方法。
技术介绍
在室内环境中，移动机器人最基本的运行模式就是沿走廊进行直线行进，由于软硬件方面的各种因素(如驱动轮子的电机调速特性不一致、车轮安装误差、地面摩擦系数不同、控制量解算误差、导航参数解算误差等)，导致机器人在沿直线轨迹行驶中会偏离设定轨迹，如果不进行纠偏则会无法完成预期功能。而在直线纠偏的过程中重要的参考信息是车辆的定位结果。使用惯性导航对机器人进行定位解算，由大量的机器人直线纠偏实验结果可知，航位推算的机器人位置信息在直线行驶的方向上精度较高，而在行进方向法线方向上解算结果与实际位置误差较大，这一误差主要来源于惯导解算中随时间误差累积的航向角。因此应当为机器人导航控制系统引入对环境中可用信息的测量，从而使用外部信息测量结果对惯性导航予以修正。
技术实现思路
本专利技术提供一种基于超声波测量的机器人室内导航方法，通过对机器人的航向角信息进行修正，提高了机器人长时间运行的可靠性。本专利技术是通过以下技术方案实现：一种基于超声波测量的机器人室内导航方法，应用于室内走廊上，其包括以下步骤：步骤1，将两个超声波模块分别固定于机器人左右两侧对称的位置上，且两个超声波模块的法线方向与地面的角度为θ；步骤2，设定机器人在室内走廊上的运行轨迹，机器人运行；k时刻，根据分别获得机器人与两侧墙壁之间的实际距离DL(k)和DR(k)；其中，DL测(k)、DR测(k)分别为左右两个超声波模块在法线方向上测得的超声波模块与墙壁之间的距离；根据Width(k)＝DL(k)+DR(k)+W获得走廊宽度Width(k)；其中W为所述机器人左右两侧对称的位置之间的宽度；步骤3，在k时刻，若航向角变化Δψ大于T1，则机器人继续按设定的运行轨迹运行；若航向角变化Δψ小于或等于T1，则执行步骤4；其中，航向角变化Δψ为当前航向与设定航向的差值；步骤4，将上一时刻的走廊宽度结果Width(k-1)与当前时刻的走廊宽度Width(k)作差，若差值大于阈值T2时，则执行步骤5；若差值小于或等于阈值T2时，则根据ΔXn(k)={12[(DL_temp-DL(k))+(DR(k)-DR_temp)]]]>计算k时刻机器人沿轨迹法向位置偏差的变化量ΔXn(k)，然后执行步骤6；其中，DL_temp及DR_temp为历史测距结果，通过前k-1个时刻两个超声波模块的测距结果分别求平均值所得；步骤5，将DL(k)与历史测距结果DL_temp进行作差，将DR(k)与历史测距结果DR_temp进行作差，如果仅有一个差值大于T3，则根据令ΔXn(k)等于大于T3的差值，然后执行步骤6；如果两个差值均大于T3，则机器人继续按设定的运行轨迹运行；如果两个差值均小于或等于T3，则根据计算k时刻机器人沿轨迹法向位置偏差的变化量ΔXn(k)，然后执行步骤6；步骤6，根据计算机器人修正后的偏向角ψUS，并根据该偏向角ψUS调整机器人的运行轨迹，是的当前运行轨迹与设定运行轨迹重合；其中，Xt(k)和Xt(k-1)是相邻两次航向角解算时刻的机器人沿轨迹切线方向位置结果，均通过DR解算获得。进一步的，所述θ为10度。进一步的，所述阈值T1为5°，阈值T2为4cm，阈值T3为3cm。与现有的技术相比，本专利技术的有益效果是：经过该算法的修正，机器人的航向角通过不断修正可以在长时间内维持在更高的精度，确保了系统的稳定运行。至此完成了机器人惯性/超声波组合导航算法的设计。附图说明图1为本专利技术的超声波模块安装及测距示意图；图2为本专利技术的超声波测距结果有效性判别流程图；图3为本专利技术的超声波测距求走廊宽度及航向角示意图；图4为本专利技术的使用超声波测距结果修正机器人航向角方法。具体实施方式为了使本专利技术的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本专利技术进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本专利技术，并不用于限定本专利技术。如图2所示，一种基于超声波测量的机器人室内导航方法，其包括以下步骤：步骤1，如图1所示，将两个超声波模块分别固定于机器人左右两侧对称的位置上，模块距地面10cm左右。为了防止模块发射的本应被墙面反射的超声波提前被地面反射，将超声波模块倾斜向上安装。测试不同安装角度下模块测量数据的稳定性及精度，最终确定安装角度为10°。步骤2，设定机器人在室内走廊上的运行轨迹，机器人运行在k时刻，根据分别获得机器人与两侧墙壁之间的实际距离DL(k)和DR(k)；其中，DL测(k)、DR测(k)分别为两个超声波模块在法线方向上测得的超声波模块与墙壁之间的距离。实时获取机器人当前航向ψ，并与设定航向ψtarget比较。当机器人当前航向ψ与设定航向ψtarget不重合时，机器人距墙壁的垂直距离应当有考虑到计算的复杂度及航向角惯性解算的精度，在程序中设定当航向角偏差Δψ＝ψ-ψtarget小于5°时，直接使用式(1)进行计算，降低计算量。根据Width(k)＝DL(k)+DR(k)+W获得走廊宽度Width(k)；其中W为机器人左右两侧对称的位置之间的宽度；步骤3，行进之前，可以控制机器人进行原地转弯，并不断采集左右两侧超声波测距结果进行分析，使其自动在初始时刻沿设定运行轨迹方向，在本申请中仅仅提出一个思考，本方案中设定机器人在初始时刻沿设定运行轨迹方向。如图3所示，在机器人顺时针原地旋转的过程中，测量的两侧障碍物距离是旋转的角度αn(机器人法线与设定航向之间的角度)的函数，即WLR(αn)。若两侧墙壁平行，则有墙体宽度Width表示为：Width＝min(WLR(αn))(2)通过WLR(αn)第一次取最小值时对应的机器人顺时针旋转角度αn可以得出旋转的初始时刻机器人航向角ψ0，即机器人行进方向相对于走廊方向的夹角。有其中机器人的航向角ψ0及旋转角度αn均以顺时针旋转为正方向，且认为机器人的初始航向ψ0与走廊方向之间的夹角不会超过90°,旋转过程中超声波采样频率越高，测量结果的精度越高，但仍受限于超声波模块的测距精度。若在k时刻，航向角变化Δψ满足|Δψ|＞T1，则判断机器人在进行大幅度转弯，不使用超声波的测距结果进行导航定位辅助解算，机器人继续按设定的运行轨迹运行；否则执行步骤4；步骤4，在机器人直行的情况下，通过存储的历史走廊宽度结果Width(k-1)与当前解算的走廊宽度结本文档来自技高网...

【技术保护点】
一种基于超声波测量的机器人室内导航方法，应用于室内走廊上，其特征在于，包括以下步骤：步骤1，将两个超声波模块分别固定于机器人左右两侧对称的位置上，且两个超声波模块的法线方向与地面的角度为θ；步骤2，设定机器人在室内走廊上的运行轨迹，机器人运行；k时刻，根据分别获得机器人与两侧墙壁之间的实际距离DL(k)和DR(k)；其中，DL测(k)、DR测(k)分别为左右两个超声波模块在法线方向上测得的超声波模块与墙壁之间的距离；根据Width(k)＝DL(k)+DR(k)+W获得走廊宽度Width(k)；其中W为所述机器人左右两侧对称的位置之间的宽度；步骤3，在k时刻，若航向角变化Δψ大于T1，则机器人继续按设定的运行轨迹运行；若航向角变化Δψ小于或等于T1，则执行步骤4；其中，航向角变化Δψ为当前航向与设定航向的差值；步骤4，将上一时刻的走廊宽度结果Width(k‑1)与当前时刻的走廊宽度Width(k)作差，若差值大于阈值T2时，则执行步骤5；若差值小于或等于阈值T2时，则根据计算k时刻机器人沿轨迹法向位置偏差的变化量ΔXn(k)，然后执行步骤6；其中，DL_temp及DR_temp为历史测距结果，通过前k‑1个时刻两个超声波模块的测距结果分别求平均值所得；步骤5，将DL(k)与历史测距结果DL_temp进行作差，将DR(k)与历史测距结果DR_temp进行作差，如果仅有一个差值大于T3，则根据令ΔXn(k)等于大于T3的差 值，然后执行步骤6；如果两个差值均大于T3，则机器人继续按设定的运行轨迹运行；如果两个差值均小于或等于T3，则根据计算k时刻机器人沿轨迹法向位置偏差的变化量ΔXn(k)，然后执行步骤6；步骤6，根据计算机器人修正后的偏向角ψUS，并根据该偏向角ψUS调整机器人的运行轨迹，使得当前运行轨迹与设定运行轨迹重合；其中，Xt(k)和Xt(k‑1)是相邻两次航向角解算时刻的机器人沿轨迹切线方向位置结果，均通过DR解算获得。...

【技术特征摘要】
1.一种基于超声波测量的机器人室内导航方法，应用于室内走廊上，其特征在于，包括
以下步骤：
步骤1，将两个超声波模块分别固定于机器人左右两侧对称的位置上，且两个超声波模
块的法线方向与地面的角度为θ；
步骤2，设定机器人在室内走廊上的运行轨迹，机器人运行；
k时刻，根据分别获得机器人与两侧墙壁之间的实际距离DL(k)
和DR(k)；其中，DL测(k)、DR测(k)分别为左右两个超声波模块在法线方向上测得的超声波模块
与墙壁之间的距离；
根据Width(k)＝DL(k)+DR(k)+W获得走廊宽度Width(k)；其中W为所述机器人左右两侧
对称的位置之间的宽度；
步骤3，在k时刻，若航向角变化Δψ大于T1，则机器人继续按设定的运行轨迹运行；若航
向角变化Δψ小于或等于T1，则执行步骤4；其中，航向角变化Δψ为当前航向与设定航向的
差值；
步骤4，将上一时刻的走廊宽度结果Width(k-1)与当前时刻的走廊宽度Width(k)作差，
若差值大于阈值T2时，则执行步骤5；若差值小于或等于阈值T2时，则根据
计算k时刻机器人沿轨迹法向位置偏差的
变化...

【专利技术属性】
技术研发人员：王玥，高鹏，张海，吕倩，王婷，吕学志，
申请(专利权)人：北京航天科工世纪卫星科技有限公司，
类型：发明
国别省市：北京;11