一种基于仿生触角的机器人自主导航方法技术领域
本发明属于智能机器人领域，具体涉及一种基于仿生触角的机器人自主导航方法。
背景技术
Bug算法是一种基于传感器的著名导航算法，它结合全局规划和局部规划的特点，其路径规划是在直接应用传感信息的基础上引入一些全局信息，但只是一种“宏观”意义上的理论算法，多运用于理论仿真，很难直接用于指导实际应用。Bug算法假设机器人为一个质点，无实体物理尺寸，并要求机器人具有全方位的障碍探测和沿着障碍物边缘绕行的能力，这一点对于实际机器人来说是极其困难的。Bug算法核心在于如何确定朝着目标点直线前进和沿着障碍物边缘绕行两种基本行走模式的切换时机以及绕行障碍物边缘的具体方式。
发明内容
有鉴于此，本发明从仿生学的角度出发，运用神经行为学原理提出了基于仿生触角的机器人自主导航算法。该方法是一种实用的改进Bug算法，综合考虑了机器人实体物理尺寸和实际环境复杂性的影响，解决了Bug算法两种基本行走模式的切换时机及绕行障碍物的具体方式，解决了Bug算法难以实际应用的问题。
为实现上述目的，本发明采用以下技术方案：
一种基于仿生触角的机器人自主导航方法，所述自主导航方法从仿生学的角度出发，提出“仿生触角”模型来理解传感器信息，将机器人简化成半径为Rr的圆，Rr为机器人外接圆半径，并包括多个检测单元。
进一步的，所述检测单元分别为半圆环形触角Dngs、Dngb和Dngo，半径分别为Rs、Rb和Ro，圆形触角Lng，箭头形触角Hng，以及位于正前方位置由矩形(2Rs×a)加半圆形(半径Rs)组成的宽面积型触角Sng，Dngs、Dngb用于检测用于探测激光雷达180°范围内最近的障碍物点；Dngo用于检测位于相遇点时障碍物所处的方位；Lng用于记录依靠自身定位系统实时得到的机器人位置；Hng用于记录航向传感器实时获得的机器人航向角；Sng用于检测当前传感器探测范围内无障碍直线行走的最大距离smax；
所述方法包括四种行走行为方式、六种行为触发条件，所述行走方式为：对准转向、直线前进、避开转向、弧线绕行，所述行为触发条件为：对准条件、偏航条件、相遇条件、避开条件、脱离条件、终点条件；
所述方法步骤如下：
步骤1：从起点S向目标点T行进，对准转向行为触发，判断是否满足对准条件，若满足对准条件，进行步骤2，不满足对准条件，继续执行对准转向行为；
步骤2:直线前行行为触发，判断是否满足偏航条件，若满足偏航条件，返回步骤1，不满足偏航条件进行步骤3；
步骤3：判断是否满足偏航条件，若满足相遇条件，进行步骤4，不满足相遇条件，进行步骤5；
步骤4：避开转向行为触发，判断是否满足避开条件，若满足避开条件，进行步骤6，不满足避开条件，继续执行避开转向行为；
步骤5：判断是否满足终点条件，若满足终点条件，终点登陆，抵达目标点T，不满足终点条件，返回步骤2；
步骤6：弧线绕行行为触发，判断是否满足相遇条件，若满足相遇条件，返回步骤4，不满足相遇条件，进行步骤7；
步骤7：判断是否满足脱离条件，若满足脱离条件，返回步骤1，不满足脱离条件，返回步骤6。
进一步的，所述四种行走行为方式具体如下：
对准转向：在S、P和L处执行的旨在对准目标点T的原地转向动作，转向方向按最小转角原则来决定，直到对准目标点T对准转向行走行为结束，其中S为起点，T为目标点，L为机器人绕行障碍物边缘结束时所处的位置点，P为机器人直线前进过程中航向角发生偏离的位置点；
直线前进：在对准目标点T后所执行的沿XT方向的直线行走动作，直到到达目标点T(此时任务结束)或者航向角发生偏离(此时位于偏离点P)或者遇到障碍物(此时位于相遇点H)为止，包括从S完成对准转向行为后的直线行走动作，航向角发生偏离后对准目标点T的直线行走动作，以及脱离障碍物边缘后对准目标点T的直线行走动作；
避开转向：在障碍物边缘绕行过程中所执行的为远离障碍物而实施的原地转向动作，当处于直线前进或弧线绕行行走行为过程中遇到障碍物，即Dngs检测到位于相遇点H，则触发避开转向行走行为，直至Dngb检测不到障碍物时避开转向行走行为结束，H表示机器人碰到障碍物时所处的位置点；
弧线绕行：在障碍物边缘绕行过程中所执行的沿一定曲率半径圆弧绕行行走动作，当遇到障碍物执行的避开转向行走行为结束时，则触发弧线绕行行走行为，直至再次遇到障碍物(Dngs检测到下一个相遇点H)或满足脱离点条件，则弧线绕行行走行为结束。
进一步的，所述避开转向行为检测过程中按施密特触发器有Rs<Rb以提高系统的稳定性，所述避开转向行为的方向确定方法是：当位于相遇点H时障碍物位于机器人左侧则原地右转，反之原地左转，如果机器人避开转向一周，则表示无法避开障碍物，停止运动，障碍物的方向通过Dngo来检测，设置Ro>Rb以提高障碍物检测的准确率，若在触角Dngo探测范围内激光雷达共检测到m个障碍物点，计算其方位角(相对于X轴)的算术平均值，记为则有：
φ ‾ = 1 m Σ i = 1 m φ i ]]>
定义如下的整型标志变量：
其中，fobt＝0表示障碍物位于右侧，fobt＝1表示障碍物位于左侧，当位于起点S和脱离点L处时，该变量进行初始化有fobt＝-1；
弧线绕行转弯方向与避开转向转弯方向对应，当遇到障碍物执行的避开转向为原地左转，则弧线右转，反之弧线左转。
进一步的，所述对准条件具体为：判断对准转向过程中机器人当前航向角是否沿着XT方向，用于连接对准转向与直线前进，对准条件通过Hng判断，其定量表达式为θr＝θXT，其中θr为机器人当前航向角，θXT表示矢线段XT的方位角，若θr＝θXT，直线前进触发，否则保持当前行为。
进一步的，所述偏航条件具体为：判断直线前进过程中机器人是否位于偏航点P，即当前航向角是否偏离XT方向，用于连接直线前进与对准转向，偏航条件通过Hng判断，其定量表达式为|θr-θXT|>θη，其中θη为预先设定的航向偏离阈值，若|θr-θXT|>θη，对准转向触发，否则保持当前行为。
进一步的，所述相遇条件具体为：判断直线前进或弧线绕行过程中是否遇到障碍物，即是否处于相遇点H，用于连接直线前进与避开转向或弧线绕行与避开转向，相遇条件通过Dngs和Dngo判断，其中Dngs判断是否遇到障碍物，Dngo判断位于相遇点H时障碍物的方位，其定量表达式为dmin≤Rs，若dmin≤Rs，避开转向触发，否则保持当前行为，其中dmin表示激光雷达探测范围内机器人与障碍物之间的最近距离。
进一步的，所述避开条件具体为：判断避开转向过程中是否避开障碍物，用于连接避开转向和弧线绕行，避开条件通过Dngb判断，其定量表达式为dmin≥Rb，若dmin≥Rb，弧线绕行触发，否则保持当前行为；当机器人避开转向一周，则表示机器人无法避开障碍物，导航任务失败。
进一步的，所述脱离条件具体为：判断在弧线绕行过程中是否脱离障碍物边缘，即判断是否处于脱离点L，用于连接弧线绕行与对准转向，脱离条件通过Hng和Sng判断，其中定量表达式为(|θSX-θXT|<θλ|||θr-θXT|≤θε&&dXT≤smax)OR(|θSX-θXT|<θλ|||θr-θXT|≤θε&&smax≥sstep)，其中θλ为预先设定的直线重合度容差，θε为预先设定的对准角度容差，θSX为矢线段SX的方位角，sstep为预先设定的脱离阈值，smax表示XT方向可无障碍直线行走的最大距离，若((|θSX-θXT|<θλ|||θr-θXT|≤θε&&dXT≤smax)OR(|θSX-θXT|<θλ|||θr-θXT|≤θε&&smax≥sstep))，对准转向触发，否则保持当前行为。
进一步的，所述终点条件具体为：判断直线前进过程中是否到达目标点T，用于判断整个导航任务是否完成，终点条件通过Lng判断，其定量表达式为dXT≤dε，其中dε为预先设定的终点距离容差，dXT表示矢线段XT的长度，若dXT≤dε，启动终点登陆程序，导航任务结束，否则保持当前行为。
本发明与现有技术相比具有如下的优点和效果：
(1)本发明引入了仿生触角的概念，在实际测量过程中，只需快速利用机载传感器(激光雷达、GPS定位系统和电子罗盘等)的大量实时数据，通过仿生触角模型对感兴趣区域的触角信息进行处理，大大减小了计算量，从而保证了导航决策的实时性。
(2)本发明设计并构建了机器人行走行为及其触发条件库，在整个导航过程中，不仅能保证机器人行走行为保持性和连贯性好，运动控制精度高，而且使得机器人行走行为之间切换自然，环境适应性好，从而提升了导航运动的稳定性。
(3)与Bug类算法抽象地要求机器人具有绕行障碍物边缘相比，本文提出了分段弧线的机制绕行障碍物边缘，不仅能忽略障碍物外形尺寸的限制，而且易于脱离障碍物边缘，运动路径短，环境适应好，从而保证了导航运动的安全性和快捷性。
附图说明
图1是机器人仿生触角模型的示意图；
图2是机器人行走行为触发条件库的示意图，其中a为对准条件，b为偏航条件，c为相遇条件，d为避开条件，e为脱离条件，f为终点条件；
图3是基于仿生触角的机器人自主导航方法的逻辑原理图；
图4是基于仿生触角的机器人自主导航方法的流程图；
图5是机器人绕行线性轮廓障碍物的自主导航示意图；
图6是机器人绕行不条件外形障碍物的自主导航示意图；
图7是机器人绕行多障碍物情形的自主导航示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细描述。应当理解，此处所描述的具体实施例仅仅用于解释本发明，并不用于限定本发明。
相反，本发明涵盖任何由权利要求定义的在本发明的精髓和范围上做的替代、修改、等效方法以及方案。进一步，为了使公众对本发明有更好的了解，在下文对本发明的细节描述中，详尽描述了一些特定的细节部分。对本领域技术人员来说没有这些细节部分的描述也可以完全理解本发明。下面结合附图与具体实施方式，对本发明进一步说明。
如图1-7所示，本发明为一种基于仿生触角的机器人自主导航方法，从仿生学的角度出发，提出“仿生触角”模型来理解传感器信息。其中激光雷达、定位传感器和航向传感器是设计仿生触角模型的主要传感器。图1为机器人仿生触角模型，将机器人简化成半径为Rr的圆(Rr为机器人外接圆半径)，并包括多个检测单元，所述检测单元为四类六个触角检测装置，分别为半圆环形触角Dngs、Dngb和Dngo(半径分别为Rs、Rb和Ro)，圆形触角Lng，箭头形触角Hng，以及位于正前方位置由矩形(2Rs×a)加半圆形(半径Rs)组成的宽面积型触角Sng。其中，Dngs、Dngb用于检测用于探测激光雷达180°范围内最近的障碍物点；Dngo用于检测位于相遇点时障碍物所处的方位；Lng用于记录依靠自身定位系统实时得到的机器人位置；Hng用于记录航向传感器实时获得的机器人航向角；Sng用于检测当前传感器探测范围内无障碍直线行走的最大距离smax。激光雷达扫描数据用极坐标形式记为(di,φi)T，其中i为扫描数据的序号。最短扫描数据di记为dmin。运用几何学知识，机器人无障碍直线行走的最大距离可由下式计算得：
s m a x = m i n { d i s i n ( φ i ) - R s 2 - d i 2 cos 2 ( φ i ) } , ( i = 1 , ... , n ) - - - ( 1 ) ]]>
其中n为位于触角Sng范围内激光雷达探测得到的障碍物点的数目，i为这些点的序号。
为叙述方便，定义以下符号：起点S，目标点T，当前位置X，用(xr,yr,θr)T表示机器人实时位姿，用dXT表示矢线段XT的长度，用θXT表示矢线段XT的方位角，用dmin表示激光雷达探测范围内机器人与障碍物之间的最近距离，用smax表示XT方向可无障碍直线行走的最大距离。(xr,yr,θr)T可由触角Lng和Hng测得，smax可由触角Sng测得。用相遇点H表示机器人碰到障碍物时所处的位置点，用脱离点L表示机器人绕行障碍物边缘结束时所处的位置点，用偏航点P表示机器人直线前进过程中航向角发生偏离的位置点。仿生触角因仅对特定感兴趣区域的信息进行处理，大大减小计算量。机器人感知的触角信息将为实现自主导航提供必要的决策判断。
本发明方法中包括如下四种机器人行走行为方式:
1)对准转向：是指在S、P和L处执行的旨在对准目标点T的原地转向动作。转向方向按最小转角原则来决定，直到对准目标点T对准转向行走行为结束。
2)直线前进：是指在对准目标点T后所执行的沿XT方向的直线行走动作，直到到达目标点T(此时任务结束)或者航向角发生偏离(此时位于偏离点P)或者遇到障碍物(此时位于相遇点H)为止，包括从S完成对准转向行为后的直线行走动作，航向角发生偏离后对准目标点T的直线行走动作，以及脱离障碍物边缘后对准目标点T的直线行走动作。
3)避开转向：是指在障碍物边缘绕行过程中所执行的为远离障碍物而实施的原地转向动作。当处于直线前进或弧线绕行行走行为过程中遇到障碍物，即Dngs检测到位于相遇点H，则触发避开转向行走行为，直至Dngb检测不到障碍物时避开转向行走行为结束。设计上按施密特触发器有Rs<Rb以提高系统的稳定性。避开转向的方向确定方法是：当位于相遇点H时障碍物位于机器人左侧则原地右转，反之原地左转。如果机器人避开转向一周，则表示无法避开障碍物，停止运动。障碍物的方向通过Dngo来检测，设计上有Ro>Rb以提高障碍物检测的准确率。假设在触角Dngo探测范围内激光雷达共检测到m个障碍物点，计算其方位角(相对于X轴)的算术平均值，记为则有：
φ ‾ = 1 m Σ i = 1 m φ i - - - ( 2 ) ]]>
定义如下的整型标志变量：
其中，fobt＝0表示障碍物位于右侧，fobt＝1表示障碍物位于左侧。当位于起点S和脱离点L处时，该变量进行初始化有fobt＝-1。
4)弧线绕行：是指在障碍物边缘绕行过程中所执行的沿一定曲率半径圆弧绕行行走动作。当遇到障碍物执行的避开转向行走行为结束时，则触发弧线绕行行走行为，直至再次遇到障碍物(Dngs检测到下一个相遇点H)或满足脱离点条件，则弧线绕行行走行为结束。弧线绕行转弯方向与避开转向转弯方向对应，当遇到障碍物执行的避开转向为原地左转，则弧线右转，反之弧线左转。如果机器人弧线绕行一圈，则表示无法脱离障碍物，停止运动。
本发明的核心在于确定机器人行走行为的触发条件。图2列出了对准条件、偏航条件、相遇条件、避开条件、脱离条件和终点条件共六种机器人行走行为触发条件。下面结合图2(a)～(f)予以说明。
1)对准条件：判断对准转向过程中机器人当前航向角是否沿着XT方向，用于连接对准转向与直线前进，见图2(a)。对准条件通过Hng判断，其定量表达式为θr＝θXT，其中θr为机器人当前航向角。若θr＝θXT，直线前进触发，否则保持当前行为，其原理用伪代码表达为：IF(θr＝θXT){直线前进触发}；ELSE{保持当前行为}。
2)偏航条件：判断直线前进过程中机器人是否位于偏航点P，即当前航向角是否偏离XT方向，用于连接直线前进与对准转向，见图2(b)。在正对着目标点直线前进过程中，由于实际机器人难免会出现车轮打滑或控制精度等问题，因此，机器人当前航向角常会出现跑偏的现象。本发明通过设置偏航条件，一旦出现跑偏现象，则立即触发对转转向，从而提高了定目标点导航的运动控制精度。偏航条件通过Hng判断，其定量表达式为|θr-θXT|>θη，其中θη为预先设定的航向偏离阈值。若|θr-θXT|>θη，对准转向触发，否则保持当前行为，其原理用伪代码表达为：IF(|θr-θXT|>θη){对准转向触发}；ELSE{保持当前行为}。
3)相遇条件Ⅰ&Ⅱ：判断直线前进或弧线绕行过程中是否遇到障碍物，即是否处于相遇点H，用于连接直线前进与避开转向或弧线绕行与避开转向，见图2(c)。相遇条件通过Dngs和Dngo判断，其中Dngs判断是否遇到障碍物，Dngo判断位于相遇点H时障碍物的方位，其定量表达式为dmin≤Rs。若dmin≤Rs，避开转向触发，否则保持当前行为，其原理用伪代码表达为:IF(dmin≤Rs){避开转向触发}；ELSE{保持当前行为}。
4)避开条件：判断避开转向过程中是否避开障碍物，用于连接避开转向和弧线绕行，见图2(d)。避开条件通过Dngb判断，其定量表达式为dmin≥Rb。若dmin≥Rb，弧线绕行触发，否则保持当前行为，其原理用伪代码表达为：IF(dmin≥Rb){弧线绕行触发}；ELSE{保持当前行为}。当机器人避开转向一周，则表示机器人无法避开障碍物，导航任务失败。
5)脱离条件：判断在弧线绕行过程中是否脱离障碍物边缘，即判断是否处于脱离点L，用于连接弧线绕行与对准转向，见图2(e)。脱离条件通过Hng和Sng判断，其中定量表达式为(|θSX-θXT|<θλ|||θr-θXT|≤θε&&dXT≤smax)OR(|θSX-θXT|<θλ|||θr-θXT|≤θε&&smax≥sstep)，其中θλ为预先设定的直线重合度容差，θε为预先设定的对准角度容差，θSX为矢线段SX的方位角，sstep为预先设定的脱离阈值。其原理用伪代码表达为：IF((|θSX-θXT|<θλ|||θr-θXT|≤θε&&dXT≤smax)OR(|θSX-θXT|<θλ|||θr-θXT|≤θε&&smax≥sstep)){对准转向触发}；ELSE{保持当前行为}。
6)终点条件：判断直线前进过程中是否到达目标点T，用于判断整个导航任务是否完成，见图2(f)。终点条件通过Lng判断，其定量表达式为dXT≤dε，其中dε为预先设定的终点距离容差。其原理用伪代码表达为：IF(dXT≤dε){启动终点登陆程序，导航任务结束}；ELSE{保持当前行为}。
至此，从仿生学的角度出发，运用神经行为学原理提出基于仿生触角的机器人自主导航方法，如图3所示，机器人依靠仿生触角感知周围环境，实时激励触发产生相应的行走行为。在整个导航过程中，机器人的行走行为具有连贯性和保持性。如果一旦某个触发条件成立，则立即触发相应的行走行为，否则继续保持当前的行走行为。
下述实例采用的机器人为长源动力XBot-520，其上安装有激光雷达、电子罗盘、GPS和驱动电机编码器等传感器。机器人定目标点导航控制过程如下：
步骤1：从起点S向目标点T行进，对准转向行为触发，判断是否满足对准条件，若满足对准条件，进行步骤2，不满足对准条件，继续执行对准转向行为；
步骤2:直线前行行为触发，判断是否满足偏航条件，若满足偏航条件，返回步骤1，不满足偏航条件进行步骤3；
步骤3：判断是否满足偏航条件，若满足相遇条件，进行步骤4，不满足相遇条件，进行步骤5；
步骤4：避开转向行为触发，判断是否满足避开条件，若满足避开条件，进行步骤6，不满足避开条件，继续执行避开转向行为；
步骤5：判断是否满足终点条件，若满足终点条件，终点登陆，抵达目标点T，不满足终点条件，返回步骤2；
步骤6：弧线绕行行为触发，判断是否满足相遇条件，若满足相遇条件，返回步骤4，不满足相遇条件，进行步骤7；
步骤7：判断是否满足脱离条件，若满足脱离条件，返回步骤1，不满足脱离条件，返回步骤6。
针对上述过程，结合图3用伪代码的形式用以说明：
【实施例一】
图5为机器人绕行线性轮廓障碍物的自主导航过程示意图。机器人从起点S处朝着目标点直线前进(由仿生触角Hng判断对准条件和偏航条件来保证机器人当前航向角始终沿着XT方向)，至相遇点H1点处遇到障碍物(由仿生触角Dngs探测到)停下来并开始进入沿着障碍物边缘绕行模式。由于仿生触角Dngo探测得到障碍物位于左侧，机器人采取避开右转和弧线左转行走行为来绕行障碍物边缘(由仿生触角Dngb判断避开条件，由仿生触角Dngs和Dngo判断相遇条件)，经过相遇点H2和H3，至脱离点L1处脱离障碍物边缘(由仿生触角Sng、Hng和Lng探测到)并开始进入朝着目标点直线前进模式，最终成功到达目标点T(由仿生触角Lng探测到)。机器人的运动轨迹为S-H1-H2-H3-L-T，由于直线型障碍物的特殊性，在脱离障碍物边缘之前每一次弧线绕行的距离相等，即有弧线段H1H2＝H2H3。弧线绕行次数与绕行弧线曲率半径Ra有关。减小Ra,弧线绕线次数增多,转向动作过于频繁；增大Ra,弧线绕行次数减少，但易使机器人忽视环境中障碍物之间的间隙而陷入“局部死区”。设计上有Ra>Rb,Ra应根据机器人车体尺寸和环境中障碍物间隙大小合理进行选取。
【实施例二】
图6为机器人绕行不条件障碍物的自主导航过程示意图。如前所述，机器人完成绕行不条件障碍物的导航过程也是依靠仿生触角感知周围环境，实时激励触发相应的行走行为来完成整个导航过程，最终机器人的运动轨迹为S-H1-L-T。由于障碍物轮廓形状的不条件性，每一次弧线绕行的弧线段长度不一定相等。因此，本发明采用分段弧线的方式绕行障碍物边缘，不仅能够忽略障碍物外形尺寸的限制，而且易于脱离障碍物边缘，运动路径短，环境适应好。
【实施例三】
图7为机器人绕行多障碍物情形的自主导航过程示意图。为简单起见，选用两个不条件外形的障碍物为例来阐述。机器人从起点S出发，经历三次朝着目标点直线前进模式和两次弧线绕行障碍物边缘模式，最终成功到达目标点T，运动轨迹为S-H1-H2-L1-H3-H4-H5-H6-L2-T。随着障碍物数量的增多，机器人绕行障碍物边缘的次数也增多。在障碍物绕行过程中，仿生触角Dngo能准确判断出障碍物所处的方位(设计上有Ro>Rb以提高障碍物方位检测的准确率)，从而保证机器人导航的运动路径全局最优。
本发明与现有技术相比具有如下的优点和效果：
(1)本发明引入了仿生触角的概念，在实际测量过程中，只需快速利用机载传感器(激光雷达、GPS定位系统和电子罗盘等)的大量实时数据，通过仿生触角模型对感兴趣区域的触角信息进行处理，大大减小了计算量，从而保证了导航决策的实时性。
(2)本发明设计并构建了机器人行走行为及其触发条件库，在整个导航过程中，不仅能保证机器人行走行为保持性和连贯性好，运动控制精度高，而且使得机器人行走行为之间切换自然，环境适应性好，从而提升了导航运动的稳定性。
(3)与Bug类算法抽象地要求机器人具有绕行障碍物边缘相比，本文提出了分段弧线的机制绕行障碍物边缘，不仅能忽略障碍物外形尺寸的限制，而且易于脱离障碍物边缘，运动路径短，环境适应好，从而保证了导航运动的安全性和快捷性。