CN113998021A - 一种仿生搜救机器人及空间自部署方法 - Google Patents

Abstract

本发明一种仿生搜救机器人及空间自部署方法，所述仿生搜救机器人节点包括弹跳机构、自复位机构、航向调节机构和传感控制单元，空间自部署方法包括路径规划算法、二维自部署算法和三维自部署算法；弹跳机构包括机架、四杆机构和驱动机构，自复位机构包括减速电机和杆腿，航向调节机构包括固定架、减速电机和调节轮；路径规划算法包括遍历临近节点方法和可行区域判定方法，二维自部署算法包括动态航向调整方法、分步调整策略和二维自部署方法，三维自部署算法包括序列点规划方法、弹跳误差与障碍物处理方法，以及三维自部署方法。所述仿生搜救机器人节点及空间自部署方法可应用于非结构复杂环境，完成监测、搜救、信号中继等任务。

Description

一种仿生搜救机器人及空间自部署方法

技术领域

本发明属于机器人学、无线传感网络、控制科学、计算机科学的交叉领域，具体为一种仿生搜救机器人及空间自部署方法。

背景技术

无线传感网络是计算机、通信和传感器三项技术结合的产物，是国内外科技领域研究的热点。通过无线传感器网络，能够实时地监测和采集其节点部署区域的各种信息(如温度、湿度、应变等)，并对这些信息进行处理后以无线的方式发送给接收者。基于无线传感器网络的系统比有线系统有着更易于部署和维护的优点，在灾害搜救、工业生产控制和环境监测等领域有着广阔的应用前景。无线传感器网络节点难以采用人工方式在危险或人员无法进入的区域进行部署。因此，无线传感器节点的自部署显得尤为重要。

在地势崎岖或者障碍物较多的环境中部署传感器节点时，传统的静态传感器节点或轮式移动传感器节点等难以在此类环境中进行移动部署。专利201220005715.2提出的仿生弹跳机器人具备良好的连续弹跳能力，专利201510262904.6提出的仿生弹跳机器人能检测障碍物的高度距离并如蝗虫般跨越高于自身数倍的障碍，但其空有弹跳能力却缺乏系统的自部署算法，无法在存在弹跳误差以及复杂障碍物环境下实现高精度的自部署。因此本发明在前专利的基础上提出了一种具备自部署功能的仿生搜救机器人，将其作为无线传感网络节点能应对复杂崎岖的地形环境，可以优化网络拓扑，提升网络生命周期。然而，仿生搜救机器人节点在自部署的过程中也面临很大的挑战。第一，仿生搜救机器人节点自身尺寸小、续航能力限制，如何实现在有限的能量下完成高精度、高完成度的的自部署；第二，仿生搜救机器人在复杂环境中如何进行路径规划，并在实际情况中如何自我调整；第三，仿生搜救机器人如何定位，在复杂环境中以一定的误差范围能自部署至设定地点。因此，本发明设计了一种以UWB技术为定位基础，根据IMU数据实现自主航向调节以及倒地自复位、以规划序列点为指导，结合弹跳误差以及障碍物检测信息进行局部动态调整的策略，实现了仿生搜救机器人节点三维环境自部署的算法。

发明内容

针对以上问题，提出一种仿生搜救机器人及空间自部署方法，解决仿生搜救机器人节点在自部署过程中的感知与位姿调节问题、仿生搜救机器人节点的离线路径规划问题，以及仿生搜救机器人节点在实际自部署的过程中如何实现自我动态调整的问题，设计了一种以UWB技术为定位基础，根据IMU数据实现自主航向调节以及倒地自复位、以规划序列点为指导，结合弹跳误差以及障碍物检测信息进行局部动态调整的策略，实现了仿生搜救机器人节点三维环境自部署的算法。

本发明一种仿生搜救机器人，包括弹跳机构、自复位机构、航向调节机构和传感控制单元；

所述的传感控制单元包括传感单元、控制处理单元以及电源单元；

所述的弹跳机构包括机架、四杆机构以及驱动机构；机架包括前端面、左端面、右端面、骨架和保护罩，左端面、右端面均固定在前端面两侧，骨架固定在左端面、右端面之间；保护罩通过螺丝固定在左端面、右端面之上；四杆机构包括大腿、小腿、大腿辅助腿以及扭力弹簧；大腿固定在机架左端面、右端面间；扭力弹簧的一个力臂抵压在大腿上，另外一个力臂抵压在机架上；大腿的末端分叉，分别与两根平置于地面、呈“八”字形的小腿铰支连接，两根小腿前端连接在一起，设置一大腿辅助腿，其两端分别与机架前端面及两根小腿的前端铰支连接；驱动机构包括弹跳电机、齿轮组以及凸轮；直流电机固定在所述机架的前端面内侧，弹跳电机的输出轴上设有齿轮，该齿轮与减速齿轮组依次啮合传动连接，减速齿轮组的末级齿轮与凸轮同轴；弹跳电机通过减速齿轮组减速后驱动凸轮，凸轮的驱动力作用在轴承上，从而驱动大腿摆动，扭力弹簧压缩存储弹性势能，在凸轮转动越过缺口处的临界点后，扭力弹簧里面的弹性势能快速释放，驱动大腿急速弹回，驱动小腿蹬地跳跃而起，从而实现跳跃功能；

所述的自复位机构包括自复位电机和调节杆腿，调节杆腿通过连接件以及顶丝与自复位电机输出轴相连，自复位电机正反转均能带动调节杆腿旋转，从而达到左右两侧均能自复位的功能；

所述的航向调节机构包括固定架、转向电机和调节轮，所述的固定架固定在小腿上；所述的转向电机与固定架相连；所述的调节轮与转向电机相连，可由转向电机驱动转动，从而实现航向调节功能。

作为本发明机器人进一步改进，所述的传感检测模块由机身九轴IMU传感器、大腿九轴IMU传感器、激光测距传感器、光照传感器以及温湿度传感器构成；所述的机身九轴IMU传感器与机架左端面相连，水平固定在机架左端面的上表面，用于检测机身的航向角、俯仰角以及横滚角信息；所述的大腿九轴IMU传感器、激光测距传感器均固定于大腿上，用于检测机器人的压缩状态以及测量与障碍物之间的距离；所述的光照传感器、温湿度传感器、定位模块以及无线通信模块均与控制处理器相连，用于监测机器人的环境状态、实现定位以及与上位机进行数据通讯。

作为本发明机器人进一步改进，所述的控制处理单元由控制处理器和数据采集存储单元组成，数据处理单元完成传感器单元的航向角、俯仰角、横滚角、测距信息、位置信息、光照信息以及温湿度信息的采集；控制处理器完成传感器单元采集数据的处理，弹跳电机、自复位电机、转向电机的驱动控制功能。

作为本发明机器人进一步改进，所述的电源单元一个锂聚合物电池，固定在机器人的大腿辅助腿内侧。

本发明提供一种仿生搜救机器人的空间自部署方法，其特征在于：所述仿生搜救机器人的空间自部署方法包括路径规划算法、二维自部署算法和三维自部署算法；

所述路径规划算法包括遍历临近节点方法和可行区域判定方法；

所述的遍历临近节点方法主要步骤如下：

步骤1：初始化开启数组和关闭数组；

步骤2：将起点加入开启数组中，并设置优先级为0，此时优先级最高；

步骤3：判断开启数组是否为空，如果不为空，则从开启数组中选取优先级最高的节点n；

步骤4：判断节点n是否为终点，如果为终点，则从终点开始逐步追踪父节点，一直达到起点，返回找到的结果路径，算法结束，如果不为终点，则将节点n从开启数组中删除，并加入关闭数组中；

步骤5：遍历节点n所有的临近节点；

步骤6：判断临近节点m是否在关闭数组中，如果是则跳过选取下个临近节点，如果不是，则设置节点m的父节点为节点n，计算节点m的优先级，将节点m加入开启数组中；

所述的可行区域判定方法具体如下：

根据仿生搜救机器人节点的空中轨迹曲线y＝xtan(Φ)-x²tan(Φ)/d+h_m，其中竖直位移为y轴，水平位移为x轴、仿生搜救机器人节点起跳角度为Φ、质心离地面距离为h_m，判断其是否能够跨越障碍物，以及跨越障碍物的状态，及将弹跳方向上的障碍物距离与高度信息带入上述公式，便可判断出障碍物是否可以跨越，并更新出障碍物影响下的实际的临近节点；

所述的二维自部署算法包括动态航向调整方法、分步调整策略和二维自部署方法；

所述的动态航向调整方法为在仿生搜救机器人节点的单跳运动中，通过UWB定位技术获取的仿生搜救机器人节点当前坐标信息与目标点坐标，求出其在基站坐标系中的预期航向；再结合基站坐标系与东北天坐标系之间的角度差，所述东北天坐标系即磁力计所处的坐标系，转化为在东北天坐标系下的航向角，最后通过磁力计不断采集偏航角来实现航向调节；

所述的分两部调整策略具体步骤如下：

步骤1：判断当前位置与目标地点之间的距离D是否小于其单跳距离，但是又大于部署误差阈值d_e，如果是，则启用分两部调整策略；

步骤2：仿生搜救机器人节点根据自身的单跳距离以及当前距离目标点的距离D，计算出第一步弹跳的方向与直面目标点的夹角δ，再结合由当前航向与直面目标点所需转动的角度θ，当前读取到的磁力计返回的航向角为φ，则将第一跳航向角调为φ₁＝φ-(θ+δ)，后单跳一次；

步骤3：再将第二步的航向角φ₂调整为：φ₂＝φ+(θ+δ)后进行一次单跳至目标点；

所述的二维自部署算法具体步骤如下：

步骤1：输入仿生搜救机器人节点坐标、部署点坐标以及设定的部署误差；

步骤2：根据当前坐标以及部署点坐标计算出需要调整的航向角；

步骤3：判断当前坐标到部署点的坐标的距离是否小于仿生搜救机器人节点单跳的跳跃距离但大于部署误差阈值d_e，如果是，则启用分两步跳策略；

步骤4：调整航向角，跳往目标点；

步骤5：判断当前坐标到部署点的坐标的距离是否小于部署误差，如果小于，则部署结束，返回部署误差，如果大于，则返回步骤2，重复上述步骤；

所述的三维自部署算法包括序列点指导跳跃方法、弹跳误差与障碍物处理方法，以及三维自部署方法；

所述的序列点指导跳跃方法是指由于障碍物的存在，3D环境中仿生搜救机器人节点的自部署需要事先获知一条可通行的前进路线，以指导自身在合理的时间以及能量消耗条件下部署到目标地点，采用所述的离线路径规划法获取可通行路径，以指导仿生搜救机器人节点的3D环境下的自部署运动，具体步骤如下:

步骤1：在经过离线路径规划之后，仿生搜救机器人节点会接收到从出发点到目的地之间的序列点；

步骤2：根据接收到的序列点，统计出总共需要弹跳的次数Count_all，此外还设置参数用于记录已跳跃的次数Count₁，以及剩余应跳的次数Count₂；

步骤3：在无特殊情况下，仿生搜救机器人节点都是朝向第Count₁+1个落点跳跃，跳跃完之后将已跳跃次数Count₁加1，直到已跳跃次数等于总共需要跳跃的次数；

所述的弹跳误差处理方法是指由于弹跳误差的存在，实际情况中有可能出现部署失败的情况；因此，在规划弹跳序列点的指导下，仿生搜救机器人节点还需要根据当前偏差对前进路线进行适当调整，以实现在合理的跳数与时间内部署到目标地点，由所述的序列点指导跳跃方法可知，仿生搜救机器人节点每一跳都朝下一个规划序列点跳跃，为了确保朝该规划序列点跳跃是合理的，其需要事先进行计算判断，设当前落点位置为(x_jk,y_jk)，该点对应的规划序列点坐标为(x_k,y_k)，则下一跳应朝规划序列点(x_k+1,y_k+1)跳跃，具体处理步骤如下：

步骤1：根据坐标值计算出仿生搜救机器人节点当前位置与对应规划序列点的平面距离L，若是L超过设定的弹跳误差阈值，则说明上一跳并未能够准确部署到规划序列点P₂附近，则需根据上一规划序列点P₁(x_k-1,y_k-1)、当前落点位置P_cur(x_jk,y_jk)以及对应规划序列点P₂(x_k,y_k)判断夹角∠P₁P₂P_cur，计算如下：

其中

为点P₁P₂之间的距离，

为点P₁P_cur之间的距离；

步骤2：若是求得夹角∠P₁P₂P_cur为锐角，则更新下一跳目标点为P₂(x_k,y_k)；

步骤3：当仿生搜救机器人节点出现超过下一规划序列点情况时，需根据下一规划序列点P₃(x_k+1,y_k+1)、当前落点位置P_cur(x_jk,y_jk)以及对应规划序列点P₂(x_k,y_k)计算夹角∠P₂P₃P_cur，计算方法如步骤1所示，若计算出距离下一目标点的距离超过误差阈值，同时角∠P₂P₃P_cur为钝角，则更新下一跳目标点为更后一个规划序列点(x_k+2,y_k+2)，若是计算得到距离下一规划落点的距离小于误差阈值，则同样更新下一跳目标为更后一个规划序列点；

所述的障碍物处理算法具体步骤如下：

步骤1：根据当前位置坐标以及目标点坐标计算出航向，并调整至该方向上；

步骤2：在航向方向上进行障碍物检测，检测该方向上障碍物距离与障碍物高度信息，若是检测出来距离大于到下一个目标点距离，则可以进行跳跃；

步骤3：若是检测出障碍物处于仿生搜救机器人当前位置与目标点之间，则需要利用检测到的障碍物距离、高度信息结合弹跳轨迹方程判断是否可以跨越障碍物，若可以直接跨越则进行跳跃，若不能跨越则计算选择短边绕行；

所述的三维自部署方法具体步骤如下：

步骤1：输入机器人坐标、规划序列点数组、部署点坐标以及设定的部署误差；

步骤2：根据规划序列点数组选择下一跳合适目标点；

步骤3：调整航向，检测前方是否有障碍物，若不存在则直接跳跃即可；

步骤4：若步骤3中检测存在障碍物，判断其高度是否能跨越，若能则直接跳跃即可；

步骤5：若步骤4中判断障碍物不可跳跃，则检测障碍物宽度；

步骤6：若步骤4中检测障碍物宽度过宽但高度在仿生搜救机器人极限跳跃高度之内，选择分两步跨越障碍物，否则计算选择短边绕行；

步骤7：判断部署误差是否达到误差阈值内，若果否则返回步骤2，循环上述步骤，如果是则返回最终部署误差，部署结束。

作为本发明仿生搜救机器人的空间自部署方法进一步改进，所述的障碍物处理算法步骤3的具体方法是：

当检测到仿生搜救机器人节点与目标点之间有障碍物无法跨越时，需要进行障碍物的宽度检测，在进行宽度检测时，得到障碍物边缘处对应的航向角和距离，设两侧方向与目标航向之间的夹角分别为α₁左侧和α₂右侧，边缘处的测距值为l₁左侧和l₂右侧，仿生搜救机器人节点与目标点之间的直线距离为l₀，障碍物边缘两侧到目标点的距离为l₃左侧和l₄右侧，通过以上信息计算得到：

然从左侧绕行的距离至少为l₁+l₃，从右侧绕行的距离至少为l₂+l₄，若是有：l₁+l₃<l₂+l₄则说明从左侧绕行距离少于从右侧绕行，此时应该选择从左侧跳跃，否则选择从右侧跳跃。

有益效果：

1、本发明设计了一种可以连续跳跃、自主航向调节、倒地自复位，具有自部署功能的仿生搜救机器人节点。

2、本发明设计了仿生搜救机器人节点的离线路径规划方法，根据已知的自部署环境可以离线规划得到一系列弹跳序列点指导放生搜救机器人节点克服障碍物并跳跃至目标地点，完成自部署。

3、本发明设计了仿生搜救机器人节点的空间自部署方法，以UWB技术为定位基础，以规划序列点为指导，结合弹跳误差以及障碍物检测信息进行局部动态调整的策略实现了仿生搜救机器人的空间自部署，可应用于非结构复杂环境，完成监测、搜救、信号中继等任务。

附图说明

图1为本发明的系统组成示意图；

图2为本发明的仿生搜救机器人节点整体机构立体视图；

图3为本发明的仿生搜救机器人节点整体机构立体视图2；

图4为本发明的仿生搜救机器人节点无障碍物环境自部署示意图；

图5为本发明的仿生搜救机器人节点障碍物只可绕行情形示意图；

图6为本发明的仿生搜救机器人节点障碍物可跨越情形示意图；

图7为本发明的仿生搜救机器人节点遇障碍物情况示意图；

图8为本发明的仿生搜救机器人节点空中轨迹图(简化模型)；

图9为本发明的仿生搜救机器人节点最短路线调整示意图；

图10为本发明的仿生搜救机器人节点往复跳跃示意图；

图11为本发明的仿生搜救机器人节点分两步跳跃示意图；

图12为本发明的仿生搜救机器人节点二维环境自部署算法流程示意图；

图13为本发明的仿生搜救机器人节点规划序列点指导下的节点弹跳示意图；

图14为本发明的仿生搜救机器人节点规划序列点夹角示意图；

图15为本发明的仿生搜救机器人节点落于下一规划落点附近的夹角示意图；

图16为本发明的仿生搜救机器人节点遇障碍物短边绕行处理方式示意图；

图17为本发明的仿生搜救机器人节点遇障碍物分步跨越处理方式示意图；

图18为本发明的仿生搜救机器人节点遇障碍物调整跨越处理方式示意图；

图19为本发明的仿生搜救机器人节点遇障碍物调整绕行处理方式示意图；

图20为本发明的仿生搜救机器人节点三维环境自部署算法流程示意图。

附件说明：

1、弹跳机构；1-1、机架；1-1-1、前端面；1-1-2、左端面；1-1-3、右端面；1-1-4、骨架；1-1-5、保护罩；1-2、四杆机构；1-2-1、大腿；1-2-2、小腿；1-2-3、大腿辅助腿；1-2-4、扭力弹簧；1-3、驱动机构；1-3-1、弹跳电机；1-3-2、齿轮组；1-3-3、凸轮；2、自复位机构；2-1、自复位电机；2-2、调节杆腿；3、航向调节机构；4、传感控制单元；4-1传感单元；4-1-1、传感检测模块；4-1-1-1、机身九轴IMU传感器；4-1-1-2、大腿九轴IMU传感器；4-1-1-3、激光测距传感器；4-1-1-4、光照传感器；4-1-1-5、温湿度传感器；4-1-2、定位模块；4-1-3、无线通信模块；4-2、控制处理单元；4-2-1、控制处理模块；4-2-2、数据处理模块；4-3、电源单元。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述：

参照附图1本发明仿生搜救机器人，包括弹跳机构1、自复位机构2、航向调节机构3和传感控制单元4。其中传感控制单元4包括传感单元4-1、控制处理单元4-2以及电源单元4-3。其中传感单元4-1包括传感检测模块4-1-1、定位模块4-1-2以及无线通信模块4-1-3。所述的传感检测模块4-1-1由机身九轴IMU传感器4-1-1-1、大腿九轴IMU传感器4-1-1-2、激光测距传感器4-1-1-3、光照传感器4-1-1-4以及温湿度传感器4-1-1-5构成。其中控制处理单元包括控制处理模块4-2-1以及数据处理模块4-2-2。

参看图2、图3，所述的弹跳机构1包括机架1-1、四杆机构1-2以及驱动机构1-3；机架1-1包括前端面1-1-1、左端面1-1-2、右端面1-1-3、骨架1-1-4和保护罩1-1-5，左端面1-1-2、右端面1-1-3均固定在前端面两侧，骨架1-1-4固定在左端面1-1-2、右端面1-1-3之间；保护罩1-1-5通过螺丝固定在左端面1-1-2、右端面1-1-3之上；四杆机构1-2包括大腿1-2-1、小腿1-2-2、大腿辅助腿1-2-3以及扭力弹簧1-2-4；大腿1-2-1固定在机架左端面1-1-3、右端面1-1-4间；扭力弹簧1-2-4的一个力臂抵压在大腿1-2-1上，另外一个力臂抵压在机架1-1上；大腿1-2-1的末端分叉，分别与两根平置于地面、呈“八”字形的小腿1-2-2铰支连接，两根小腿1-2-2前端连接在一起，设置一大腿辅助腿1-2-3，其两端分别与机架前端面1-1-1及两根小腿1-2-2的前端铰支连接；驱动机构1-3包括弹跳电机1-3-1、齿轮组1-3-2以及凸轮1-3-3；直流电机1-3-1固定在所述机架的前端面1-1-1内侧，弹跳电机1-3-1的输出轴上设有齿轮，该齿轮与减速齿轮组1-3-2依次啮合传动连接，减速齿轮组的末级齿轮与凸轮1-3-3同轴；弹跳电机1-3-1通过减速齿轮组1-3-2减速后驱动凸轮1-3-3，凸轮1-3-3的驱动力作用在轴承上，从而驱动大腿1-2-1摆动，扭力弹簧1-2-4压缩存储弹性势能，在凸轮1-3-3转动越过缺口处的临界点后，扭力弹簧1-2-4里面的弹性势能快速释放，驱动大腿1-2-1急速弹回，驱动小腿1-2-2蹬地跳跃而起，从而实现跳跃功能。

参看图2、图3，所述的自复位机构2包括自复位电机2-1和调节杆腿2-2，调节杆腿2-2通过连接件以及顶丝与自复位电机2-1输出轴相连，自复位电机2-1正反转均能带动调节杆腿2-2旋转，从而达到左右两侧均能自复位的功能。

参看图2、图3，所述的航向调节机构3包括固定架3-1、转向电机3-2和调节轮3-3，所述的固定架3-1固定在小腿1-2-2上；所述的转向电机3-2与固定架相连；所述的调节轮3-3与转向电机3-2相连，可由转向电机3-2驱动转动，从而实现航向调节功能。

参看图1、图2、图3，所述的传感控制单元4包括传感单元4-1、控制处理器单元4-2以及电源单元4-3；所述的传感单元4-1包括传感检测模块4-1-1、定位模块4-1-2以及无线通信模块4-1-3。所述的传感检测模块4-1-1由机身九轴IMU传感器4-1-1-1、大腿九轴IMU传感器4-1-1-2、激光测距传感器4-1-1-3、光照传感器4-1-1-4以及温湿度传感器4-1-1-5构成；所述的机身九轴IMU传感器4-1-1-1与机架左端面1-1-2相连，水平固定在机架左端面1-1-2的上表面，用于检测机身的航向角、俯仰角以及横滚角信息；所述的大腿九轴IMU传感器4-1-1-2、激光测距传感器4-1-1-3均固定于大腿1-2-1上，用于检测机器人的压缩状态以及测量与障碍物之间的距离；所述的光照传感器4-1-1-4、温湿度传感器4-1-1-5、定位模块4-1-2以及无线通信模块4-1-3均与控制处理器4-2-1相连，用于监测机器人的环境状态、实现定位以及与上位机进行数据通讯。

所述的控制处理单元4-2由控制处理器4-2-1和数据采集存储单元4-2-2组成，数据处理单元4-2-2完成传感器单元的航向角、俯仰角、横滚角、测距信息、位置信息、光照信息以及温湿度信息的采集；控制处理器4-2-1完成传感器单元4-1采集数据的处理，弹跳电机1-3-1、自复位电机2-1、转向电机3-2的驱动控制功能。

所述的电源单元4-3一个锂聚合物电池，固定在机器人的大腿辅助腿1-2-3内侧。

下面结合附图对仿生搜救机器人的空间自部署方法作进一步说明。

为了在有限时间和跳数内到达目标地点，仿生搜救机器人节点需要具备合理的自部署策略。参看图4，对于无障碍物的环境，在所示四锚点UWB定位坐标4-a中仿生搜救机器人节点4-b部署只需要朝向目标地点4-d跳跃，并且在每一次跳跃落地后重新调整航向，不断朝目标点方向跳跃4-c直到到达部署点指定范围内。参看图5，对于有障碍物环境5-e，在所示四锚点UWB定位坐标5-a中，则事先进行路径规划，得到一组可行的规划路径用于指导仿生搜救机器人节点5-b在实际中的部署运动5-d至目标点5-c。针对传统的平面移动机器人，路径规划过程中遇到障碍物只能选择绕行。参看图6，在所示四锚点UWB定位坐标6-a中，仿生搜救机器人节点6-b凭借优越的跨障能力，还具备跨越障碍物6-e的选择6-d，更高效地部署至目标点6-c。

本发明中，将自部署策略分解为两个部分，第一部分是路径规划，即根据已知的自部署环境，通过离线路径规划得到一系列弹跳序列点。第二部分为自部署调整，主要为仿生搜救机器人节点依据路径规划得到的一系列弹跳序列点运动，由于弹跳误差等的存在，在部署过程需要进行相应调整。

所述的路径规划方法为根据已知的环境地图以及地图中的障碍物信息，利用一定算法构造出从起点到终点的可行路径。本发明基于A*算法进行改进研究，并结合仿生搜救机器人节点离散运动以及跳跃式运动的特点，进行符合弹跳式运动的序列点规划设计。主要包括遍历临近节点算法以及可行区域判定算法。

所述的遍历临近节点算法具体步骤如下：

步骤1：初始化开启数组和关闭数组；

步骤2：将起点加入开启数组中，并设置优先级为0(优先级最高)；

步骤3：判断开启数组是否为空，如果不为空则从开启数组中选取优先级最高的节点n；

步骤4：判断节点n是否为终点，如果为终点，则从终点开始逐步追踪父节点，一直达到起点，返回找到的结果路径，算法结束，如果不为终点，则将节点n从开启数组中删除，并加入关闭数组中；

步骤5：遍历节点n所有的临近节点；

步骤6：判断临近节点m是否在关闭数组中，如果是则跳过选取下个临近节点，如果不是，则设置节点m的父节点为节点n，计算节点m的优先级，将节点m加入开启数组中；

参看图7、在仿生搜救机器人节点遇到障碍物时，主要出现以下几类情况，一种是跳跃到空中时会碰撞障碍物，导致前进失败7-1；一种是跳上障碍物7-2，一种是跳过障碍物7-3，另一种是从障碍物上跳下7-4。所述的可行区域判定方法为根据仿生搜救机器人节点的空中轨迹曲线y＝xtan(Φ)-x²tan(Φ)/d+h_m(竖直位移为y轴，水平位移为x轴、仿生搜救机器人节点起跳角度为Φ、质心离地面距离为h_m)判断其是否能够跨越障碍物，以及跨越障碍物的状态。及将弹跳方向上的障碍物距离与高度信息带入上述公式，便可判断出障碍物是否可以跨越，并更新出障碍物影响下的实际的临近节点。

所述的空间自部署自部署方法包括二维自部署算法以及三维自部署算法。

所述的二维自部署算法包括动态航向调整方法、分步调整策略和二维自部署方法。

所述的动态航向调整方法为在仿生搜救机器人的单跳运动中，通过UWB定位技术获取的仿生搜救机器人节点当前坐标信息与目标点坐标，可以求出在基站坐标系中的预期航向；再结合基站坐标系与东北天坐标系(磁力计所处的坐标系)之间的角度差，转化为在东北天坐标系下的航向角，最后通过磁力计不断采集偏航角来实现航向调节。参看图9，仿生搜救机器人节点计算出自身位置9-1距离目标地点9-2的距离D大于一跳的距离d_j时，则按照上述计算，将朝向旋转θ度后，使其面朝目标地点跳跃。不断执行该步骤，直到检测到距离目标点的距离小于设定的部署误差阈值d_e，则返回信息告知上位机已经到达目的地点9-3。

仿生搜救机器人节点在部署接近目标地点的过程中，可能出现计算出的与目标地点之间的距离D小于一次跳跃的距离 ，但是又大于部署误差阈值d_e。此时需要对该情况进行讨论。

当出现上述情况时，仿生搜救机器人节点会根据当前距离D以及跳跃距离d_j预估出下一个落点与目标点的距离d:d≈d_j-D

若是d<d_e，仿生搜救机器人节点则选择与之前直线跳跃一般，面朝目标地点跳跃。

若是d>d_e，参看图10若仿生搜救机器人节点10-1还是笔直朝着目标地点10-2跳跃，则落地后与目标点之间的距离D很大可能是超过d_e的，即并未达到部署误差范围内。下一跳则会调转航向，朝着上一点的方向前进，并落在上一跳附近，此后在两点间来回跳跃，难以部署成功。因此，在这种情况下需要进行特殊处理，改变部署的策略。本发明采用的是分两步跳跃的策略，当检测到d>d_e的情况发生，就改变方向，以分两跳走折线的方式向目标地点前进，参看图11。

所述的分两部调整策略具体步骤如下：

步骤1：判断当前位置11-1与目标地点11-3之间的距离D是否小于一次跳跃的距离，但是又大于部署误差阈值d_e，如果是，则启用分两部调整策略。

步骤2：仿生搜救机器人节点根据自身的单跳距离以及当前距离目标点的距离D，计算出第一步弹跳的方向与直面目标点的夹角δ，再结合由当前航向与直面目标点所需转动的角度θ，当前读取到的磁力计返回的航向角为φ，则将第一跳航向角调为φ₁＝φ-(θ+δ)，后单跳一次。

步骤3：再将第二步的航向角φ₂调整为：φ₂＝φ+(θ+δ)后进行一次单跳至目标点11-3。

参看图12，所述的二维自部署算法具体步骤如下：

步骤S1：输入机器人坐标、部署点坐标以及设定的部署误差；

步骤S2：根据计算当前坐标到部署点坐标的距离D；

步骤S3：判断D小于仿生搜救机器人节点单跳的跳跃距离但大于部署误差阈值d_e，如果是，则启用分两步跳策略；

步骤S4：调整航向角，跳往目标点；

步骤S5：判断当前坐标到部署点的坐标的距离是否小于部署误差，如果小于，则部署结束，返回部署误差，如果大于，则返回步骤2，重复上述步骤。

所述的三维自部署算法包括序列点指导跳跃方法、弹跳误差与障碍物处理方法，以及三维自部署方法。

所述的序列点指导跳跃方法是指由于障碍物的存在，3D环境中仿生搜救机器人节点的自部署需要事先获知一条可通行的前进路线，以指导自身在合理的时间以及能量消耗条件下部署到目标地点。采用所述的离线路径规划法获取可通行路径，以指导仿生搜救机器人节点的3D环境下的自部署运动。参看图13，Point_k-1到Point_k+2代表离线路径规划获取的弹跳序列落点。仿生搜救机器人初始位置13-1处于Point_k-1，每一跳均朝向下一个规划落点的方向进行跳跃13-2、13-3，直到到达目标地点。具体步骤如下:

步骤1：在经过离线路径规划之后，仿生搜救机器人节点会接收到从出发点到目的地之间的序列点。

步骤2：根据接收到的序列点，统计出总共需要弹跳的次数Count_all，此外仿生搜救机器人节点还设置参数用于记录已跳跃的次数Count₁，以及剩余应跳的次数Count₂。

步骤3：在无特殊情况下，仿生搜救机器人节点都是朝向第Count₁+1个落点跳跃，跳跃完之后将已跳跃次数Count₁加1，直到已跳跃次数等于总共需要跳跃的次数。

所述的弹跳误差处理方法是指由于弹跳误差的存在，仿生搜救机器人节点有可能随着弹跳误差的累计出现部署失败的情况。因此，在规划弹跳序列点的指导下，还需要根据当前偏差以及前方障碍物信息对前进路线进行适当调整，以实现在合理的跳数与时间内部署到目标地点。由所述的序列点指导跳跃方法可知，仿生搜救机器人节点每一跳都朝下一个规划序列点跳跃，为了确保朝该规划序列点跳跃是合理的，需要事先进行计算判断。设当前落点位置为(x_jk,y_jk)，该点对应的规划序列点坐标为(x_k,y_k)，则正常情况下下一跳应朝规划序列点(x_k+1,y_k+1)跳跃。具体处理步骤如下：

步骤1：参看图14，根据坐标值可以计算出当前位置与对应规划序列点的平面距离L。随着弹跳误差的累计，若是L超过设定的弹跳误差阈值d，则说明上一跳并未能够准确部署到规划序列点P₂附近。接下来需要根据上一规划序列点P₁(x_k-1,y_k-1)、当前落点位置P_cur(x_jk,y_jk)以及对应规划序列点P₂(x_k,y_k)判断夹角∠P₁P₂P_cur，计算如下：

其中

为点P₁P₂之间的距离，

为点P₁P_cur之间的距离。

步骤2：若是求得夹角∠P₁P₂P_cur为锐角，则更新下一跳目标点为P₂(x_k,y_k)。

步骤3：参看图15，当仿生搜救机器人节点出现超过下一规划序列点情况时，需根据下一规划序列点P₃(x_k+1,y_k+1)、当前落点位置P_cur(x_jk,y_jk)以及对应规划序列点P₂(x_k,y_k)计算夹角∠P₂P₃P_cur，计算方法如步骤1所示，若计算出距离下一目标点的距离超过误差阈值，同时角∠P₂P₃P_cur为钝角，则更新下一跳目标点为更后一个规划序列点P₄(x_k+2,y_k+2)。若是计算得到距离下一规划落点的距离L小于误差阈值d，则同样更新下一跳目标为更后一个规划序列点。

所述的障碍物处理算法具体步骤如下：

步骤1：根据当前位置坐标以及目标点坐标计算出航向，并调整至该方向上。

步骤2：在航向方向上进行障碍物检测，检测该方向上障碍物距离与障碍物高度信息，若是检测出来距离大于到下一个目标点距离，则仿生搜救机器人可以进行跳跃。

步骤3：若是检测出障碍物处于仿生搜救机器人当前位置与目标点之间，则需要利用检测到的障碍物距离、高度信息结合弹跳轨迹方程判断是否可以跨越障碍物，若可以直接跨越则进行跳跃，若不能跨越则计算选择短边绕行。

进一步地，步骤3的具体方法是：

参看图16，当检测到仿生搜救机器人节点16-1与目标点之间16-3有障碍物无法跨越时，仿生搜救机器人节点需要进行障碍物的宽度检测。在进行宽度检测时，可以得到障碍物边缘处对应的航向角和距离。设两侧方向与目标航向之间的夹角分别为α₁和α₁，边缘处的测距值为l₁和l₂，目标点之间的直线距离为l₀，障碍物边缘到目标点的距离为l₃和l₄。通过以上信息可计算得到：

显然从左侧绕行的距离至少为l₁+l₃，从右侧绕行的距离至少为l₂+l₄，若是有：l₁+l₃<l₂+l₄则说明从左侧绕行距离少于从右侧绕行，此时应该选择从左侧跳跃16-2，否则选择从右侧跳跃。

参看图17，若是检测到障碍物的宽度较大，此时利用绕行方式将会耗费较多跳数与能量，若是障碍物高度是小于仿生搜救机器人节点17-1最大跳跃高度，则可以利用分两步策略17-2、17-3调整至合适的起跳位置，采用跨越障碍物的方式前往目标点17-4。

参看图18，若是目标点18-4处于障碍物上，而实际检测障碍物信息发现无法一次跨越上去时，此时先进行障碍物宽度检测。若是目标点即为最终部署点，那么仿生搜救机器人节点18-1只能采用分两步策略18-2、18-3调整到合适的位置，然后跳跃到障碍物上目标点18-4。若是仍有后续规划序列点且后续目标点18-5又处于平面，则此时如上文所述计算从障碍物短边绕行到下一个目标点的距离与预估跳数，若是绕行到下一目标点所需要的跳数大于于跨越式则可选择跨越。反之参看图19，仿生机器人节点19-1计算选择障碍物短边绕行19-2、19-3至下一目标点19-4。

参看图20，所述的三维自部署方法具体步骤如下：

步骤S1：输入机器人坐标、规划序列点数组、部署点坐标以及设定的部署误差；

步骤S2：根据规划序列点数组选择下一跳合适目标点；

步骤S3：调整航向，检测前方是否有障碍物，若不存在则直接跳跃即可；

步骤S4：若步骤3中检测存在障碍物，判断其高度是否能跨越，若能则直接跳跃即可；

步骤S5：若步骤4中判断障碍物不可跳跃，则检测障碍物宽度；

步骤S6：若步骤4中检测障碍物宽度过宽但高度在仿生搜救机器人极限跳跃高度之内，选择分两步跨越障碍物，否则计算选择短边绕行；

步骤S7：判断部署误差是否达到误差阈值内，若果否则返回步骤2，循环上述步骤，如果是则返回最终部署误差，部署结束。

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。

Claims (6)

1.一种仿生搜救机器人，包括弹跳机构(1)、自复位机构(2)、航向调节机构(3)和传感控制单元(4)，其特征在于，

所述的传感控制单元(4)包括传感单元(4-1)、控制处理单元(4-2)以及电源单元(4-3)；

所述的弹跳机构(1)包括机架(1-1)、四杆机构(1-2)以及驱动机构(1-3)；机架(1-1)包括前端面(1-1-1)、左端面(1-1-2)、右端面(1-1-3)、骨架(1-1-4)和保护罩(1-1-5)，左端面(1-1-2)、右端面(1-1-3)均固定在前端面两侧，骨架(1-1-4)固定在左端面(1-1-2)、右端面(1-1-3)之间；保护罩(1-1-5)通过螺丝固定在左端面(1-1-2)、右端面(1-1-3)之上；四杆机构(1-2)包括大腿(1-2-1)、小腿(1-2-2)、大腿辅助腿(1-2-3)以及扭力弹簧(1-2-4)；大腿(1-2-1)固定在机架左端面(1-1-3)、右端面(1-1-4)间；扭力弹簧(1-2-4)的一个力臂抵压在大腿(1-2-1)上，另外一个力臂抵压在机架(1-1)上；大腿(1-2-1)的末端分叉，分别与两根平置于地面、呈“八”字形的小腿(1-2-2)铰支连接，两根小腿(1-2-2)前端连接在一起，设置一大腿辅助腿(1-2-3)，其两端分别与机架前端面(1-1-1)及两根小腿(1-2-2)的前端铰支连接；驱动机构(1-3)包括弹跳电机(1-3-1)、齿轮组(1-3-2)以及凸轮(1-3-3)；直流电机(1-3-1)固定在所述机架的前端面(1-1-1)内侧，弹跳电机(1-3-1)的输出轴上设有齿轮，该齿轮与减速齿轮组(1-3-2)依次啮合传动连接，减速齿轮组的末级齿轮与凸轮(1-3-3)同轴；

所述的自复位机构(2)包括自复位电机(2-1)和调节杆腿(2-2)，调节杆腿(2-2)通过连接件以及顶丝与自复位电机(2-1)输出轴相连；

所述的航向调节机构(3)包括固定架(3-1)、转向电机(3-2)和调节轮(3-3)，所述的固定架(3-1)固定在小腿(1-2-2)上；所述的转向电机(3-2)与固定架相连；所述的调节轮(3-3)与转向电机(3-2)相连。

2.根据权利要求1所述的一种仿生搜救机器人，其特征在于：所述的传感检测模块(4-1-1)由机身九轴IMU传感器(4-1-1-1)、大腿九轴IMU传感器(4-1-1-2)、激光测距传感器(4-1-1-3)、光照传感器(4-1-1-4)以及温湿度传感器(4-1-1-5)构成；所述的机身九轴IMU传感器(4-1-1-1)与机架左端面(1-1-2)相连，水平固定在机架左端面(1-1-2)的上表面，用于检测机身的航向角、俯仰角以及横滚角信息；所述的大腿九轴IMU传感器(4-1-1-2)、激光测距传感器(4-1-1-3)均固定于大腿(1-2-1)上，用于检测机器人的压缩状态以及测量与障碍物之间的距离；所述的光照传感器(4-1-1-4)、温湿度传感器(4-1-1-5)、定位模块(4-1-2)以及无线通信模块(4-1-3)均与控制处理器(4-2-1)相连，用于监测机器人的环境状态、实现定位以及与上位机进行数据通讯。

3.根据权利要求1所述的一种仿生搜救机器人，其特征在于：所述的控制处理单元(4-2)由控制处理器(4-2-1)和数据采集存储单元(4-2-2)组成，数据处理单元(4-2-2)完成传感器单元的航向角、俯仰角、横滚角、测距信息、位置信息、光照信息以及温湿度信息的采集；控制处理器(4-2-1)完成传感器单元(4-1)采集数据的处理，弹跳电机(1-3-1)、自复位电机(2-1)、转向电机(3-2)的驱动控制功能。

4.根据权利要求1所述的一种仿生搜救机器人，其特征在于：所述的电源单元(4-3)一个锂聚合物电池，固定在机器人的大腿辅助腿(1-2-3)内侧。

5.一种仿生搜救机器人的空间自部署方法，其特征在于：所述仿生搜救机器人的空间自部署方法包括路径规划算法、二维自部署算法和三维自部署算法；

所述路径规划算法包括遍历临近节点方法和可行区域判定方法；

所述的遍历临近节点方法主要步骤如下：

步骤1：初始化开启数组和关闭数组；

步骤2：将起点加入开启数组中，并设置优先级为0，此时优先级最高；

步骤3：判断开启数组是否为空，如果不为空，则从开启数组中选取优先级最高的节点n；

步骤4：判断节点n是否为终点，如果为终点，则从终点开始逐步追踪父节点，一直达到起点，返回找到的结果路径，算法结束，如果不为终点，则将节点n从开启数组中删除，并加入关闭数组中；

步骤5：遍历节点n所有的临近节点；

步骤6：判断临近节点m是否在关闭数组中，如果是则跳过选取下个临近节点，如果不是，则设置节点m的父节点为节点n，计算节点m的优先级，将节点m加入开启数组中；

所述的可行区域判定方法具体如下：

根据仿生搜救机器人节点的空中轨迹曲线y＝xtan(Φ)-x²tan(Φ)/d+h_m，其中竖直位移为y轴，水平位移为x轴、仿生搜救机器人节点起跳角度为Φ、质心离地面距离为h_m，判断其是否能够跨越障碍物，以及跨越障碍物的状态，及将弹跳方向上的障碍物距离与高度信息带入上述公式，便可判断出障碍物是否可以跨越，并更新出障碍物影响下的实际的临近节点；

所述的二维自部署算法包括动态航向调整方法、分步调整策略和二维自部署方法；

所述的动态航向调整方法为在仿生搜救机器人节点的单跳运动中，通过UWB定位技术获取的仿生搜救机器人节点当前坐标信息与目标点坐标，求出其在基站坐标系中的预期航向；再结合基站坐标系与东北天坐标系之间的角度差，所述东北天坐标系即磁力计所处的坐标系，转化为在东北天坐标系下的航向角，最后通过磁力计不断采集偏航角来实现航向调节；

所述的分两部调整策略具体步骤如下：

步骤1：判断当前位置与目标地点之间的距离D是否小于其单跳距离，但是又大于部署误差阈值d_e，如果是，则启用分两部调整策略；

步骤2：仿生搜救机器人节点根据自身的单跳距离以及当前距离目标点的距离D，计算出第一步弹跳的方向与直面目标点的夹角δ，再结合由当前航向与直面目标点所需转动的角度θ，当前读取到的磁力计返回的航向角为φ，则将第一跳航向角调为φ₁＝φ-(θ+δ)，后单跳一次；

步骤3：再将第二步的航向角φ₂调整为：φ₂＝φ+(θ+δ)后进行一次单跳至目标点；

所述的二维自部署算法具体步骤如下：

步骤1：输入仿生搜救机器人节点坐标、部署点坐标以及设定的部署误差；

步骤2：根据当前坐标以及部署点坐标计算出需要调整的航向角；

步骤3：判断当前坐标到部署点的坐标的距离是否小于仿生搜救机器人节点单跳的跳跃距离但大于部署误差阈值d_e，如果是，则启用分两步跳策略；

步骤4：调整航向角，跳往目标点；

步骤5：判断当前坐标到部署点的坐标的距离是否小于部署误差，如果小于，则部署结束，返回部署误差，如果大于，则返回步骤2，重复上述步骤；

所述的三维自部署算法包括序列点指导跳跃方法、弹跳误差与障碍物处理方法，以及三维自部署方法；

所述的序列点指导跳跃方法是指由于障碍物的存在，3D环境中仿生搜救机器人节点的自部署需要事先获知一条可通行的前进路线，以指导自身在合理的时间以及能量消耗条件下部署到目标地点，采用所述的离线路径规划法获取可通行路径，以指导仿生搜救机器人节点的3D环境下的自部署运动，具体步骤如下:

步骤1：在经过离线路径规划之后，仿生搜救机器人节点会接收到从出发点到目的地之间的序列点；

步骤2：根据接收到的序列点，统计出总共需要弹跳的次数Count_all，此外还设置参数用于记录已跳跃的次数Count₁，以及剩余应跳的次数Count₂；

步骤3：在无特殊情况下，仿生搜救机器人节点都是朝向第Count₁+1个落点跳跃，跳跃完之后将已跳跃次数Count₁加1，直到已跳跃次数等于总共需要跳跃的次数；

所述的弹跳误差处理方法是指由于弹跳误差的存在，实际情况中有可能出现部署失败的情况；因此，在规划弹跳序列点的指导下，仿生搜救机器人节点还需要根据当前偏差对前进路线进行适当调整，以实现在合理的跳数与时间内部署到目标地点，由所述的序列点指导跳跃方法可知，仿生搜救机器人节点每一跳都朝下一个规划序列点跳跃，为了确保朝该规划序列点跳跃是合理的，其需要事先进行计算判断，设当前落点位置为(x_jk,y_jk)，该点对应的规划序列点坐标为(x_k,y_k)，则下一跳应朝规划序列点(x_k+1,y_k+1)跳跃，具体处理步骤如下：

步骤1：根据坐标值计算出仿生搜救机器人节点当前位置与对应规划序列点的平面距离L，若是L超过设定的弹跳误差阈值，则说明上一跳并未能够准确部署到规划序列点P₂附近，则需根据上一规划序列点P₁(x_k-1,y_k-1)、当前落点位置P_cur(x_jk,y_jk)以及对应规划序列点P₂(x_k,y_k)判断夹角∠P₁P₂P_cur，计算如下：

其中

为点P₁P₂之间的距离，

为点P₁P_cur之间的距离；

步骤2：若是求得夹角∠P₁P₂P_cur为锐角，则更新下一跳目标点为P₂(x_k,y_k)；

步骤3：当仿生搜救机器人节点出现超过下一规划序列点情况时，需根据下一规划序列点P₃(x_k+1,y_k+1)、当前落点位置P_cur(x_jk,y_jk)以及对应规划序列点P₂(x_k,y_k)计算夹角∠P₂P₃P_cur，计算方法如步骤1所示，若计算出距离下一目标点的距离超过误差阈值，同时角∠P₂P₃P_cur为钝角，则更新下一跳目标点为更后一个规划序列点(x_k+2,y_k+2)，若是计算得到距离下一规划落点的距离小于误差阈值，则同样更新下一跳目标为更后一个规划序列点；

所述的障碍物处理算法具体步骤如下：

步骤1：根据当前位置坐标以及目标点坐标计算出航向，并调整至该方向上；

步骤2：在航向方向上进行障碍物检测，检测该方向上障碍物距离与障碍物高度信息，若是检测出来距离大于到下一个目标点距离，则可以进行跳跃；

步骤3：若是检测出障碍物处于仿生搜救机器人当前位置与目标点之间，则需要利用检测到的障碍物距离、高度信息结合弹跳轨迹方程判断是否可以跨越障碍物，若可以直接跨越则进行跳跃，若不能跨越则计算选择短边绕行；

所述的三维自部署方法具体步骤如下：

步骤1：输入机器人坐标、规划序列点数组、部署点坐标以及设定的部署误差；

步骤2：根据规划序列点数组选择下一跳合适目标点；

步骤3：调整航向，检测前方是否有障碍物，若不存在则直接跳跃即可；

步骤4：若步骤3中检测存在障碍物，判断其高度是否能跨越，若能则直接跳跃即可；

步骤5：若步骤4中判断障碍物不可跳跃，则检测障碍物宽度；

步骤6：若步骤4中检测障碍物宽度过宽但高度在仿生搜救机器人极限跳跃高度之内，选择分两步跨越障碍物，否则计算选择短边绕行；

步骤7：判断部署误差是否达到误差阈值内，若果否则返回步骤2，循环上述步骤，如果是则返回最终部署误差，部署结束。

6.根据权利要求1所述的一种仿生搜救机器人的空间自部署方法，其特征在于：

所述的障碍物处理算法步骤3的具体方法是：

当检测到仿生搜救机器人节点与目标点之间有障碍物无法跨越时，需要进行障碍物的宽度检测，在进行宽度检测时，得到障碍物边缘处对应的航向角和距离，设两侧方向与目标航向之间的夹角分别为α₁左侧和α₂右侧，边缘处的测距值为l₁左侧和l₂右侧，仿生搜救机器人节点与目标点之间的直线距离为l₀，障碍物边缘两侧到目标点的距离为l₃左侧和l₄右侧，通过以上信息计算得到：

然从左侧绕行的距离至少为l₁+l₃，从右侧绕行的距离至少为l₂+l₄，若是有：l₁+l₃<l₂+l₄则说明从左侧绕行距离少于从右侧绕行，此时应该选择从左侧跳跃，否则选择从右侧跳跃。

Images