本发明属于自动化控制技术改进领域，尤其涉及一种基于毫米波雷达的无人机避障方法。
背景技术：
随着农业自动化技术的快速发展，植保无人机在农作物洒药领域得到了广泛的应用。植保无人机的实际农田作业环境非常复杂，电线杆、树、人、动物、地势差等都会成为无人机飞行过程中的障碍物，特别是农田中常见的电线杆，提高了无人机炸机的风险。为了避开障碍物，要求用户在规划航线时有意地避开地图中的障碍物，这容易导致地图分块，增加工作量，降低了无人机的作业效率。特别是在障碍物位置未知的情况下，航线无法准确、可靠地规划，导致无人机无法作业。
现有技术中，基于图像的自主避障方法是在摄像头识别到障碍物时进行悬停并避开障碍物。该类方案无法完全克服夜晚低光飞行对摄像头成像的影响，只能降低速度飞行，导致夜间作业效率降低。基于雷达的自主避障方法则遇到障碍物物后悬停，无法继续自主飞行，必须依赖人为操控避开障碍物，作业效率大大降低。
技术实现要素：
本发明的目的在于提供一种基于毫米波雷达的无人机避障方法，旨在解决的问题。
本发明是这样实现的，一种基于毫米波雷达的无人机避障方法，所述无人机避障方法包括以下步骤：
s1、用户通过地面站设置无人机作业区域并自动生成作业航线；
s2、通过毫米波雷达获取当前环境中障碍物与自动飞行作业中无人机的距离信息和角度信息并以无人机为中心建立局部栅格地图；
s3、根据获取的距离和角度信息计算障碍物与无人机的相对位置并依据rtk模块给出的飞行坐标计算障碍物的绝对坐标；
s4、根据无人机的实时位置获取的障碍物的绝对坐标映射在以实时无人机为中心的局部栅格地图上；
s5、以无人机为中心螺旋式搜索障碍物坐标点是否大于预设值，如大于等于预设值，则将障碍物置于备选区并执行下一步；如小于预设值。则删除该信息；
s6、根据无人机既定作业航线判断当前障碍物是否影响当前航线飞行，如是，则根据航线方向计算无人机与障碍物之间的位置信息生成避障线路航行并执行下一步，如否，则继续航行工作；
s7、判断无人机是否避开所述当前障碍物，如是，则返回原作业航线继续工作并执行步骤s5，如否，则继续航形并执行步骤s7。
本发明的进一步技术方案是：所述步骤s4中还包括以下步骤：
s41、判断毫米波雷达反馈的每个数据信息是否真实有效，如有效，则将有效数据映射在栅格地图上，如否，则对当前数据不做任何处理。
本发明的进一步技术方案是：所述步骤s5中还包括以下步骤：
s51、根据障碍物的优先级指标筛选出对无人接航线影响最大的障碍物。
本发明的进一步技术方案是：所述步骤s2中的采用多个毫米波雷达组合进行探测，在无人机左右两侧倾斜一定角度安装宽波束毫米波雷达覆盖两侧区域，在无人机前方安装窄波束毫米波雷达检测无人机前方高风险区域。
本发明的进一步技术方案是：所述窄波束毫米波雷达进行远距离检测并指示无人机减速，所述宽波束毫米波雷达对两侧重叠区负责高位区域的数据冗余检测并对两侧区域远距离探测及近距离障碍物精确定位。
本发明的进一步技术方案是：所述宽波束毫米波雷达减速策略为：判断障碍物位于无人机左右两侧的安全距离以内，如在安全距离以内，则控制无人机减速至水平距离等于安全距离处悬停，如在安全距离以外，则判断垂直距离是否大于可疑距离阈值，如垂直距离小于可疑距离阈值，则无人机在距离障碍物一定距离处开始减速至较小的设定速度继续飞行，直至无人机越过可疑障碍物后恢复原作业速度继续作业，如垂直距离大于可疑距离阈值，则无人机不做响应，以作业速度继续飞行。
本发明的进一步技术方案是：所述窄波束毫米波雷达减速策略为：在障碍物位于波束范围内，当检测到障碍物时，计算无人机与障碍物的距离并在设定预设减速距离处开始减速，降低至预设速度后保持匀速飞行。当宽波束毫米波雷达检测到障碍物并减速移动至设定范围内，立即清除原障碍物信息并开始第二次确认，如计时结束前未检测到该障碍物，则恢复原作业速度继续作业，如第二次确认检测到障碍物，则自动规划避障路线开始避障。当窄波束雷达和宽波束雷达同时探测到不同位置的障碍物时，减速优先级为：宽波束雷达监测的近距离障碍物＞窄波束监测的远距离障碍物＞宽波束雷达监测到的远距离障碍物＞窄波束监测的近距离障碍物。
本发明的进一步技术方案是：所述步骤s3中所述障碍物的绝对坐标的计算公式为：
xo＝xp+dr·cos(psi+θr+α)+dp·cos(psi)
yo＝yp+dr·sin(psi+θr+α)+dp·sin(psi)
其中，(xo，yo)为障碍物的坐标，(xp，yp)为无人机位置做坐标；dr为毫米波雷达输出的障碍物距离；psi为无人机航向角度，θr为毫米波雷达输出的障碍物角度；α为雷达安装角度；dp为毫米波雷达中心与无人机中心的距离。
本发明的进一步技术方案是：所述并在s6中的减速优先级为宽波束雷达监测的近距离障碍物＞窄波束监测的远距离障碍物＞宽波束雷达监测到的远距离障碍物＞窄波束监测的近距离障碍物。
本发明的进一步技术方案是：所述并在s6中的避障线路根据障碍物到无人机的水平距离和垂直距离生成；所述避障线路受作业区域限制不允许超出作业区域，在优先的避障线路方向上超过作业区域则尝试生成另一侧方向的避障路线。
本发明的有益效果是：利用雷达反馈检测障碍物，建立避障路线，有效的提高了飞行速度，解除了人为操控避开障碍物，提高了作业效率，增加了夜晚作业的工作效率。
附图说明
图1是本发明实施例提供的基于毫米波雷达的无人机避障方法的流程图。
图2是本发明实施例提供的局部栅格地图示意图。
图3是本发明实施例提供的常规自动避障路线生成示意图。
图4是本发明实施例提供的去程航线边界附近有障碍物时，自动避障路线生成示意图。
图5是本发明实施例提供的作业航线边界附近有障碍物时，自动避障路线生成示意图。
具体实施方式
如图1所示，本发明提供的基于毫米波雷达的无人机避障方法流程图，其详述如下：
步骤s1，用户通过地面站设置无人机作业区域并自动生成作业航线；用户在地面设置的地面站内对无人机进行作业区域设置，是利用平面坐标系在无人机控制系统中设置在一定的区域内进行工作，并根据区域的坐标值与无人机所覆盖的范围内自动生成飞行航线，其中雷达采用的是毫米波雷达。
步骤s2，通过毫米波雷达获取当前环境中障碍物与自动飞行作业中无人机的距离信息和角度信息并以无人机为中心建立局部栅格地图；无人机在飞行作业中，安装在其正前方和两侧的雷达在不停的发出探测声波，通过反馈的探测声波获取当前环境中遇到障碍物与无人机之间的距离和角度信息，在次过程中还会以无人机为中心实时的建立局部栅格地图，如图2所示。
采用多个毫米波雷达组合进行探测，在无人机左右两侧倾斜一定角度安装宽波束毫米波雷达覆盖两侧区域，在无人机前方安装窄波束毫米波雷达检测无人机前方高风险区域。
所述窄波束毫米波雷达进行远距离检测并指示无人机减速，所述宽波束毫米波雷达对两侧重叠区负责高位区域的数据冗余检测并对两侧区域远距离探测及近距离障碍物精确定位。
所述宽波束毫米波雷达减速策略为：判断障碍物位于无人机左右两侧的安全距离以内，如在安全距离以内，则控制无人机减速至水平距离等于安全距离处悬停，如在安全距离以外，则判断垂直距离是否大于可疑距离阈值，如垂直距离小于可疑距离阈值，则无人机在距离障碍物一定距离处开始减速至较小的设定速度继续飞行，直至无人机越过可疑障碍物后恢复原作业速度继续作业，如垂直距离大于可疑距离阈值，则无人机不做响应，以作业速度继续飞行。
所述窄波束毫米波雷达减速策略为：在障碍物位于波束范围内，当检测到障碍物时，计算无人机与障碍物的距离并在预设减速距离处开始减速，降低至预设速度后保持匀速飞行，。当宽波束毫米波雷达检测到障碍物并减速移动至设定范围内，立即清除原障碍物信息并开始第二次确认，如计时结束前未检测到该障碍物，则恢复原作业速度继续作业，如第二次确认检测到障碍物，则自动规划避障路线开始避障。当窄波束雷达和宽波束雷达同时探测到不同位置的障碍物时，减速优先级为：宽波束雷达监测的近距离障碍物＞窄波束监测的远距离障碍物＞宽波束雷达监测到的远距离障碍物＞窄波束监测的近距离障碍物。
步骤s3，根据获取的距离和角度信息计算障碍物与无人机的相对位置并依据rtk模块给出的飞行坐标计算障碍物的绝对坐标；在飞行的过程中，控制系统根据雷达反馈的信息计算出障碍物与无人机之间的相对位置关系，在根据卫星定位模块给出的经纬度信息转换成的飞行坐标计算障碍物的绝对坐标，所述障碍物的绝对坐标的计算公式为：
xo＝xp+dr·cos(psi+θr+α)+dp·cos(psi)
yo＝yp+dr·sin(psi+θr+α)+dp·sin(psi)
其中，(xo，yo)为障碍物的坐标，(xp，yp)为无人机位置做坐标；dr为毫米波雷达输出的障碍物距离；psi为无人机航向角度，θr为毫米波雷达输出的障碍物角度；α为雷达安装角度；dp为毫米波雷达中心与无人机中心的距离。
步骤s4，根据无人机的实时位置获取的障碍物的绝对坐标映射在以实时无人机为中心的局部栅格地图上；判断毫米波雷达反馈的每个数据信息是否真实有效，如有效，则将有效数据映射在栅格地图上，如否，则对当前数据不做任何处理。判断毫米波雷达的每一个数据是否真实有效，并将有效数据输入栅格地图。实时根据无人机的位置计算栅格地图的坐标，更新地图数据。
步骤s5，以无人机为中心螺旋式搜索障碍物坐标点是否大于预设值，如大于等于预设值，则将障碍物置于备选区并执行下一步；如小于预设值。则删除该信息；以无人机为中心，螺旋式地向外扫描地图，将地图内的障碍物放入障碍物备选区，根据障碍物优先指标筛选出对无人机航线影响最大的障碍物。其中，筛选优先级为垂直距离小于安全垂直距离且直线距离最近的障碍物＞垂直距离小于安全垂直距离但直线距离较远的障碍物＞垂直距离略大于安全垂直距离的障碍物＞垂直距离远大于安全垂直距离的障碍物
步骤s6，根据无人机既定作业航线判断当前障碍物是否影响当前航线飞行，如是，则根据航线方向计算无人机与障碍物之间的位置信息生成避障线路航行并执行下一步，如否，则继续航行工作；避障线路根据障碍物到无人机的水平距离和垂直距离生成；所述避障线路受作业区域限制不允许超出作业区域，在优先的避障线路方向上超过作业区域则尝试生成另一侧方向的避障路线。
步骤s7，判断无人机是否避开所述当前障碍物，如是，则返回原作业航线继续工作并执行步骤s5，如否，则继续航形并执行步骤s7。结合已规划好的植保无人机的作业航线判断该障碍物是否影响当前航线的飞行，根据航线方向计算无人机与障碍物的垂直距离和水平距离，生成避障线路自主避开障碍物。待无人机避开障碍物并返回航线后沿原航线继续作业，防止漏喷现象。具体避障路线实例如图3至图5所示。
采用多个毫米波雷达组合进行探测。在无人机的左右两侧以一定角度倾斜安装宽波束毫米波雷达，用于覆盖无人机左右两侧的区域。同时，在无人机前方安装窄波束毫米波雷达，用于检测无人机前方的高风险区域。
正前方的窄波束毫米波雷达主要用于远距离检测并指示无人机减速；左右两侧的宽波束毫米波雷达的重叠区负责高危区域的数据冗余检测。同时，可分别负责无人机左右区域的远距离探测和近距离障碍物精确定位。
其中，所述宽波束雷达采用以下策略减速：
如果障碍物位于无人机左右侧安全距离以内，即障碍物与当前航线的垂直距离小于安全距离，控制无人机减速至水平距离等于安全距离处悬停。
如果障碍物位于无人机两侧安全距离以外，且垂直距离小于可疑距离阈值，无人机在距离障碍物一定距离处开始减速至较小的设定速度继续飞行，直至无人机越过可疑障碍物后恢复原作业速度继续作业。
如果障碍物位于无人机两侧，且垂直距离大于可疑距离阈值，则无人机不做响应，以作业速度继续飞行。
所述窄波束雷达采用以下策略减速：
如果障碍物位于波束范围内，默认该障碍物位于航线的正前方。当检测到障碍物时，计算飞机与障碍物的距离，在设定减速距离处开始减速，直至速度降低至设定速度时，匀速飞行。如果宽波束雷达检测到障碍物并进入二次检测环节或者越过障碍物为止，恢复原作业速度继续作业。
避障系统依据各雷达探测到的障碍物的优先级进行减速操作。优选的，可设置优先级为：宽波束雷达监测的近距离障碍物＞窄波束监测的远距离障碍物＞宽波束雷达监测到的远距离障碍物＞窄波束监测的近距离障碍物。
使用rtk定位定向系统用于测量无人机的实时位置和航向。
如图2所示，所述障碍物坐标计算公式为：
xo＝xp+dr·cos(psi+θr+α)+dp·cos(psi)；yo＝yp+dr·sin(psi+θr+α)+dp·sin(psi)；
所述(xo，yo)为障碍物的坐标，(xp，yp)为无人机位置做坐标；dr为毫米波雷达输出的障碍物距离；psi为无人机航向角度，θr为毫米波雷达输出的障碍物角度；α为雷达安装角度；dp为毫米波雷达中心与无人机中心的距离。
无人机刹车至障碍物前方后，进行障碍物二次检测。
如果障碍物再次被毫米波雷达检测到，无人机按照预设的避障逻辑避开障碍物。
如果障碍物未被毫米波雷达检测到，无人机恢复原作业速度继续作业。
使用舵机驱动毫米波雷达在无人机俯仰方向旋转，舵机依据无人机的俯仰角信息实时调整雷达角度，确保雷达始终与水平面呈一定角度。
采用惯导传感器测量毫米波雷达的实时倾斜角度，反馈给雷达数据处理模块，用于控制舵机旋转。当无人机处于加减速飞行状态时，其俯仰角为负，此时惯导传感器测得该俯仰角并通过pwm信号控制舵机反方向转动，补偿俯仰角对雷达的影响，雷达始终朝着无人机正前方区域探测，有效避免了地面信号的干扰，提高了毫米波雷达的信号稳定性。
所述雷达数据处理模块用于雷达数据的获取和处理、雷达倾斜角度数据处理、舵机信号的输出以及与飞控的数据通信。
前向雷达和左右侧雷达分别建议独立的栅格地图。其中左右侧的雷达共用一个二维地图，该地图是以无人机为中心的正方形区域，该地图每个栅格的坐标采用取整的方式计算得到，地图内的数据无需依据无人机飞行方向和无人机小范围运动频繁更新。同时，兼顾数据的可移动性和较低的数据运算成本；前向雷达对应的地图为一维栅格，地图的方向与当前航线一致，地图的起始栅格为无人机坐标取整后的结果。地图内的数据无需依据无人机小范围运动频繁更新，地图数据会在切换航线后清除。
栅格地图内的障碍物均对应一个概率值，该值与雷达探测到该位置的次数呈正相关；如果某一栅格的概率值大于设定的阈值时，在地图中标记出该障碍物位置。优选地，选择每个栅格的计数值的三次方为概率值，这样，所述栅格的概率值随着计数值的增加呈现指数增长，代表该栅格内存在障碍物的概率较大。
采用毫米波雷达输出的障碍物速度vo和无人机速度vp数据匹配的方式滤除无关虚警数据，具体地，可以采用以下方滤除无效数据：
如果：vo≤vp+δv，并且vo≥vp-δv，并且vo＜0时，雷达数据与无人机速度匹配，数据可输入栅格地图；
如果：vo＞vp+δv，或者vo＜vp-δv，或者vo≥0时，雷达数据与无人机速度不匹配，数据被滤除，不可输入栅格地图。
其中，δv是与速度误差范围有关的参数。
地图栅格搜索采用以无人机为中心的螺旋式由近向远搜索的方式。以下为搜索实例：以无人机所在栅格为第一格进行搜索，待栅格搜索完成，并且没有障碍物时，搜索该区域右侧一格。待栅格搜索完成，并且没有障碍物时，搜索该区域上侧一格。待栅格搜索完成，并且没有障碍物时，搜索该区域左侧两格。待栅格内搜索完成，并且没有障碍物时，搜索该区域下侧三格。以此类推逆时针方向不断遍历栅格地图，当遇到障碍物所在栅格，将该栅格中心坐标作为障碍物位置放入备选区。直至备选区内障碍物大于最大障碍物数量后搜索停止。
避障路径依据障碍物到无人机的水平距离和垂直距离生成，以规定好的安全距离避开障碍物。优选的，避障路线的第一条线路与障碍物所在方向相反，避免过长的避障线路降低作业效率。
同时，无人机生成的避障路线需受到作业区域的限制，避障线路不允许超出作业区域。如果避障线路在优选的方向上越过了作业区域，所述避障系统尝试生成另一个方向的避障线路，防止路径搜索失败。对于障碍物在航线边界处的情况，避障系统选择性跳过部分航线直接进入下一航线继续作业，有效地避开障碍物所在危险区域。
避障过程中无人机停止作业，防止重复洒药。无人机避开障碍物返回原定航线后，开启作业系统继续作业，防止漏喷。
无人机生成有效的避障路线后，沿避障路线飞行过程中如果毫米波雷达监测到有其他障碍物，控制所述无人机悬停。此时，地面站弹出报警信息告知用户。
无人机刹车至障碍物前方后，重新进行障碍物第二次检测。如果障碍物再次被毫米波雷达检测到，无人机按照预定的避障逻辑避开障碍物；如果障碍物未被毫米波雷达检测到，无人机按照原作业速度继续作业。
使用舵机驱动毫米波雷达在无人机俯仰方向旋转，舵机依据无人机的俯仰角信息实时调整雷达角度，确保雷达始终与水平面呈一定角度。
采用惯导传感器测量毫米波雷达的实时倾斜角度，反馈给雷达数据处理模块，用于控制舵机旋转。
所述雷达数据处理模块用于雷达数据的获取和处理、雷达倾斜角度数据处理、舵机信号的输出以及与飞控的数据通信。
前向雷达和左右侧雷达分别建立独立的栅格地图。其中左右侧的两个雷达共用一个二维栅格地图，该地图是以无人机为中心的正方形区域，该地图每个栅格的坐标采用取整的方式计算得到，地图内的数据无需依据无人机飞行方向和无人机小范围运动频繁更新。前向雷达对应的地图为一维栅格地图，地图的方向与当前航线一致，地图的起始栅格为无人机坐标取整后的结果。地图内的数据无需依据无人机小范围运动频繁更新，地图内的数据会在每次切换航线后清除。
栅格地图内的障碍物均对应一个概率值，该值与雷达探测到该位置的次数呈正相关；如果某一栅格的概率值大于设定的阈值时，在地图中标记出该障碍物位置。
采用毫米波雷达输出的障碍物速度信息和无人机速度数据匹配的方式滤除无关虚警数据。
地图栅格搜索采用以无人机为中心的螺旋式由近向远搜索的方式。
避障路径依据障碍物到无人机的水平距离和垂直距离生成，以规定好的安全距离避开障碍物。
无人机生成的避障路线需受到作业区域的限制，避障线路不允许超出作业区域。
避障过程中无人机停止作业，防止重复洒药。无人机避开障碍物返回原定航线后，开启作业系统继续作业，防止漏喷。
无人机生成有效的避障路线后，沿避障路线飞行过程中如果毫米波雷达监测到有其他障碍物，控制所述无人机悬停。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。