一种控制单旋翼植保无人机避障飞行的方法与流程
本发明属于无人机避障技术领域，尤其涉及一种控制单旋翼植保无人机避障飞行的方法。
背景技术：
农业植保无人机在美国、俄罗斯、日本等国运用的非常普遍，技术也十分先进，然而中国作为农业大国，18亿亩基本农田，农业植保无人机却尚未得到普及。但是，我国的农业植保无人机市场正值上升阶段，正因为尚未普及所以市场大有作为。中国每年需要大量的人员从事农业植保作业，而我国每年农药中毒人数有10万之众，同时，农村青壮年劳动力逐渐稀缺，人力成本日益增加。植保无人机则可远距离遥控操作，避免了喷洒作业人员暴露于农药的危险，保障了喷洒作业的安全。
植保无人机，是用于农林植物保护作业的无人驾驶飞机，该型无人飞机由飞行平台、gps飞控、喷洒机构三部分组成，通过地面遥控或gps飞控，来实现喷洒作业，可以喷洒药剂、种子、粉剂等。植保无人机采用高效无刷电机作为动力，机身振动小，可以搭载精密仪器，喷洒农药等更加精准；同时，植被地形要求低，作业不受海拔限制；植保无人机起飞调校短、效率高、出勤率高；植保无人机具有环保，无废气，符合国家节能环保和绿色有机农业发展要求。
植保无人机不断普及，开始走进普通老百姓的生活。但是植保无人机由于没有采用相关的防碰撞防护，导致植保无人机在作业过程中，增加了很多的限制，如作业田块周界10米范围内要求无人员居住的房舍，作业田块周界10米范围内无防护林、高压线塔、电杆等障碍物，作业田块中间无影响飞行安全的障碍物或影响飞行视线的障碍物，作物高度应低于操作人员的视线，操作人员能够观察到飞机飞行姿态等要求，这些限制将会使得植保无人机的使用受到一定的地域限制，不利于植保无人机的全面普及。
技术实现要素：
本发明提供了一种控制单旋翼植保无人机避障飞行的方法，根据无人机与障碍物的相对距离、无人机与障碍物的相对速度、无人机与地面的高度进行相应的控制。
本申请的技术方案是：一种控制单旋翼植保无人机避障飞行的方法，包括：
p1,首先判断无人机与障碍物的相对距离，将相对距离进行三个部分的划分:小于n1m，n1m到n2m，n2m到n3m三个距离范围；
p2,距离划分完成后，根据无人机与障碍物的相对速度进行危险等级的划分:
p3,然后判断无人机与地面的高度值，将该高度值h进行等级的划分；
p4：对于危险等级，需要进行步骤p3的操作；对于警示等级，需要紧急减速后进行步骤p3的操作；对于提示等级和不相关等级，不用进行第三步骤的判断，返回重新进行检测。
进一步的，步骤p2具体为：当距离小于n1m，速度大于m1m/s,且预警时间小于qs，则属于危险等级，速度小于m1m/s时，属于警示等级；
当距离n1m≤r<n2m，速度大于m2m/s时,处于危险等级；当速度m1m/s≤v<m2m/s时，处于警示等级，当速度小于m1m/s时，处于提示等级；
当距离n2m≤r<n3m时，当速度大于m3m/s时,处于危险等级；当速度m2m/s≤v<m3m/s时，处于警示等级，当速度m1m/s≤v<m2m/s时，处于提示等级，当速度小于m1m/s时，处于不相关等级；
进一步的，步骤p3具体为：当高度0m≤h<50m，根据毫米波防撞雷达传感器进行无人机与障碍物方位角的判断，如果障碍物处于无人机的左边，则控制无人机进行右偏飞行，躲避障碍物；如果障碍物处于无人机的右边，控制无人机进行左偏飞行，躲避障碍物；如果障碍物处于无人机的正前方，则无人机爬升进行避障；
当高度h≥50m，根据毫米波防撞雷达进行无人机与障碍物方位角的判断，如果障碍物处于无人机的左边，则控制无人机进行右偏飞行，躲避障碍物，如果障碍物处于无人机的右边，控制无人机进行左偏飞行，躲避障碍物；如果障碍物处于无人机飞行的正前方，采用紧急悬停后进行折返避障。
进一步的，上述方法还包括：在进行步骤p1前，数据采集层使用各个传感器对无人机飞行环境中的障碍物数据进行采集：
1)毫米波防撞雷达传感器输出数据为无人机与障碍物的相对距离r1、相对速度v1、障碍物与雷达法线之间的角度θ1；
2)超声波传感器采集无人机与障碍物的相对距离r2；
3)毫米波雷达高度表输出无人机与地面的相对高度值h1；
4)gps/北斗定位传感器输出无人机对地的海拔高度值h2；
5)ahrs模块输出三维加速度a_x,a_y,a_z、三维角速度w_x,w_y,w_z和三维地磁场强度m_x,m_y,m_z，通过上述数据解算出无人机当前的姿态数据。
进一步的，所述姿态数据，包括飞行方位角θ2、俯仰角ψ1和翻滚角
更进一步的，在进行数据采集之后，数据预处理层进行异常数据处理和数据缺失补偿两个步骤；
异常数据处理的步骤为：首先在采集到的各个传感器数据中找出异常值，采用求平均移动线的方法，进行异常数据处理；设含有异常数据序列为{xi,i＝1,2,λ}，当i＝n时出现异常值，则用移动平均线xn替代，即利用i＝n之前的n个数据取其均值计算出xn；计算公式如下：
更进一步的，数据缺失补偿的步骤为：设含有数据缺失序列为{xi,i＝1,2,λ}，数据长度n，缺失数据点的前n个点的检测值的序列为xn,n＝1,2,λ,n，需要平滑处理的步数m；
首先进行一次指数平滑计算，计算公式为：s′n＝αxn+(1-α)s′n-1，其中s′n为第一次平滑后的第n个数据均值，s′n-1为第一次平滑后的第n-1个数据均值，s′n-1的计算公式为：
然后进行二次指数平滑计算，计算公式为：s″n＝αs′n+(1-α)s″n-1，其中s″n为第二次平滑后的第n个数据均值，s″n-1为第二次平滑后的第n-1个数据均值，s″n-1的计算公式为：
最后是进行三次指数平滑计算，计算公式为：s″′n＝αs″n+(1-α)s″′n-1，其中s″′n为第三次平滑后的第n个数据均值，s″′n-1为第三次平滑后的第n-1个数据均值，s″′n-1的计算公式为：
xn+m＝an+bnm+cnm2/2，其中，xn+m为第n+m个数据的预测值，m为由n开始向后平滑处理的步数，α取0.5，an，bn，cn的计算公式如下所示：
作为进一步的，在数据预处理之后，数据融合层进行无人机与障碍物相对距离的数据融合：
a、距离在0m到1m范围内，对超声波雷达传感器采集的数据进行kalman数据融合；
b、距离在1m到10m范围内,采用超声波雷达传感器和毫米波防撞雷达传感器进行检测，然后加权平均，即引入α的加权值对超声波雷达传感器和毫米波防撞雷达传感器进行加权平均，将加权融合后的数据进行kalman数据融合；
c、距离在10m到50m范围内,直接对毫米波防撞雷达传感器采集的数据进行kalman数据融合。
作为更进一步的，根据不同的实际距离，各个传感器采集计算的距离如下：
作为更进一步的，数据融合层进行无人机与地面相对高度的数据融合，毫米雷达高度表和gps/北斗定位传感器所获取的无人机高度值进行数据融合，高度值的数据融合根据距离分为两种；
对于高度小于50m的范围内，采用毫米雷达高度表和gps/北斗定位传感器对无人机高度进行检测，检测后的结果采用加权平均，即引入α值对这两种传感器的高度值进行加权平均处理，处理后采用kalman对高度值进行数据融合；根据ahrs航姿数据对高度值进行修正：
对于高度大于50m以上，采用gps/北斗定位传感器，将获取的高度数据直接进行kalman数据融合，然后，采用ahrs航姿数据进行高度修正；
其中h1是毫米雷达高度表采集的高度，h2是gps/北斗定位传感器采集的高度。
本发明由于采用以上技术方案，能够取得如下的技术效果：本申请能够更好地实现单旋翼植保无人机对植保环境的感知，获得更加准确的障碍物数据信息，做出更准确的避障决策以及做出避障控制。多传感器数据融合是把在不同位置的多个同类或不同类传感器所提供的局部环境的不完整数据加以综合，消除传感器间可能存在的冗余和矛盾的数据，加以互补，降低其不确定性，以形成对系统环境的相对完整一致的感知描述，从而提高智能系统的决策、规划、反映的快速性和正确性，降低决策风险。对于测量无人机与障碍物之间的距离，超声波测距、双目识别以及毫米波雷达传感器，三种传感器在测量距离时都有各自的优点和不足，将三者合理、有效结合，则可对相对距离的测量实现有效的互补，提高测量距离数据的准确获得。同理，对于无人机与障碍物的相对速度以及无人机与地面高度值都是利用各个传感器的优缺点，互相弥补，进行数据融合，以提高数据精度，改善系统可靠性。
单旋翼植保无人机系统通过合理支配和充分使用多个传感器的数据资源，使得多传感器在时间和空间上的冗余或互补信息进行组合处理，以获得环境以及障碍物目标特征获取的一致性的描述。
附图说明
为了更清楚的说明本发明的实施例或现有技术的技术方案，下面将对实施例或现有技术描述中所需要使用的附图做一简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为实施例1中检测装置结构框图；
图2一种控制单旋翼植保无人机避障飞行的方法结构框图；
图3为实施例中数据融合层进行无人机与障碍物相对距离的数据融合流程图；
图4为实施例中数据融合层进行无人机与地面相对高度的数据融合流程图；
图5为决策层控制无人机的示意图；
图6为实施例中决策层完成避障的流程图。
具体实施方式
为使本发明的实施例的目的、技术方案和优点更加清楚，下面结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚完整的描述：
本申请是针对单旋翼的植保无人机，单旋翼植保无人机旋翼大，飞行稳定，抗风条件好，风场稳定，雾化效果好，下旋风力大，穿透力强，农药可以打到农作物的根茎部位；核心部件为进口电机，构件为航空铝材，碳纤材料，结实耐用，性能稳定；飞行植保作业历时2年多，连续植保10万亩/次以上，无重大故障，飞行驾驶简便易学，稳定智能的飞控系统，傻瓜式的操作模式经过简单培训就可上手。
实施例1
本实施例提供了一种控制单旋翼植保无人机避障飞行的方法，包括：数据采集层、数据预处理层、数据融合层、决策层和检测装置；
所述检测装置，包括：
毫米波雷达高度表，对无人机与地面的相对高度进行测量；
gps/北斗定位传感器，采集定位信息、无人机海拔高度、无人机飞行速度等；
ahrs模块，采集无人机飞行姿态数据；
毫米波防撞雷达传感器，采集无人机与障碍物的相对速度、相对距离以及方位角等；
超声波雷达传感器，用于对障碍物的相对距离进行采集；
主控制器，通过对各个传感器获得的数据进行分析，控制无人机完成避障动作；
所述主控制器分别与毫米波雷达高度表、gps/北斗定位传感器、ahrs模块、毫米波防撞雷达传感器、超声波雷达传感器相连。
可选地，所述毫米波防撞雷达传感器、超声波雷达传感器安装在无人机前方，超声波雷达传感器进行0～10米范围的测距，毫米波防撞雷达传感器进行1～50m范围内的测距。
可选地，ahrs模块，包括mems的三轴陀螺仪、加速度计和磁强计，输出数据为三维加速度、三维角速度和三维地磁场强度。
实施例2
作为对实施例1的补充，数据采集层使用各个传感器对无人机飞行环境中的障碍物数据进行采集：
1)毫米波防撞雷达传感器输出数据为无人机与障碍物的相对距离r1、相对速度v1、障碍物与雷达法线之间的角度θ1，即(r1,v1,θ1)；
2)超声波传感器采集无人机与障碍物的相对距离r2；
3)毫米波雷达高度表输出无人机与地面的相对高度值h1；
4)gps/北斗定位传感器输出无人机对地的海拔高度值h2；
gps数据遵循nmea0183协议，所输出的信息都是标准的具有固定格式的。其中与无人机导航密切相关的有gpgga和gpvtg语句。它们的数据格式规定如下：
(1)$gpgga，utc时间，纬度，纬度半球，经度，经度半球，gps模式，星数，hdop水平精度因子，海拔高度，m，地球椭球面相对大地水平面的高度，m，差分时间，差分站id*hh<cr><lf>。
(2)$gpvtg，以真北为基准的地面航向，t，以磁北为基准的地面航向，m，地面速率(节)，n，地面速率(公里/小时)，k，模式指示*hh<cr><lf>。通过提取gpgga语句中，特定位置的海拔高度数据，可以得到无人机的海拔高度h2。
5)ahrs模块输出三维加速度a_x,a_y,a_z、三维角速度w_x,w_y,w_z和三维地磁场强度m_x,m_y,m_z，通过上述数据解算出无人机当前的姿态数据，即飞行方位角θ2、俯仰角ψ1和翻滚角
实施例3
由于数据在采集过程中可能会存在数据缺失等情况，以及后续数据进行融合处理时，需要提前进行数据集成等工作，因此需要进行数据预处理等工作；所述数据预处理层，包括：异常数据处理和数据缺失补偿两个步骤；
异常数据的存在会大大影响预测的准确性；因此提供一种异常数据处理的步骤为：首先在采集到的各个传感器数据中找出异常值，采用求平均移动线的方法，进行异常数据处理；设含有异常数据序列为{xi,i＝1,2,λ}，当i＝n时出现异常值，则用移动平均线xn替代，即利用i＝n之前的n个数据取其均值计算出xn；计算公式如下：
数据的丢失会严重影响后续的数据处理的准确性和连续性。在采集到的各个传感器的数据中可能在若干长度的连续数据或某个数据点出现数据缺失的问题，而且对于各个传感器采集到的数据可能构成具有2次、3次或高次的数据模型，所以需要用高次平滑形式。因此对于所有缺失的若干长度的连续数据或某个数据点，采用布朗3次指数平滑法进行处理。布朗3次指数平滑法由从线性平滑再进行1次平滑过渡到2次多项式平滑，即是3次平滑。本发明采用布朗3次指数平滑法对缺失的数据进行平滑处理。
数据缺失补偿的步骤为：设含有数据缺失序列为{xi,i＝1,2,λ}，数据长度n，缺失数据点的前n个点的检测值的序列为xn,n＝1,2,λ,n，需要平滑处理的步数m；
首先进行一次指数平滑计算，计算公式为：s′n＝αxn+(1-α)s′n-1，其中s′n为第一次平滑后的第n个数据均值，s′n-1为第一次平滑后的第n-1个数据均值，s′n-1的计算公式为：
然后进行二次指数平滑计算，计算公式为：s″n＝αs′n+(1-α)s″n-1，其中s″n为第二次平滑后的第n个数据均值，s″n-1为第二次平滑后的第n-1个数据均值，s″n-1的计算公式为：
最后是进行三次指数平滑计算，计算公式为：s″′n＝αs″n+(1-α)s″′n-1，其中s″′n为第三次平滑后的第n个数据均值，s″′n-1为第三次平滑后的第n-1个数据均值，s″′n-1的计算公式为：
xn+m＝an+bnm+cnm2/2，其中，xn+m为第n+m个数据的预测值，m为由n开始向后平滑处理的步数，α取0.5，an，bn，cn的计算公式如下所示：
实施例4
本实施例针对数据融合层进行详细讲述，数据融合层进行无人机与障碍物相对距离的数据融合：
a、距离在0m到1m范围内，超声波传感器采集到的相对距离值精度更高，毫米波防撞雷达传感器对于1m范围内会存在一定的盲区，所以在这区间范围的数据使用超声波雷达传感器采集的数据进行kalman数据融合；
b、距离在1m到10m范围内,采用超声波雷达传感器和毫米波防撞雷达传感器进行检测，所以在该距离范围内，然后加权平均，即引入α的加权值对超声波雷达传感器和毫米波防撞雷达传感器进行加权平均，将加权融合后的数据进行kalman数据融合；
c、距离在10m到50m范围内,已经超出超声波雷达的测距范围，但是毫米波防撞雷达传感器还可以进行检测，所以在该距离范围内，直接对毫米波防撞雷达传感器采集的数据进行kalman数据融合；
即如下所示
数据融合层进行无人机与地面相对高度的数据融合，毫米雷达高度表和gps/北斗定位传感器所获取的无人机高度值进行数据融合，高度值的数据融合根据距离分为两种；
对于高度小于50m的范围内，采用毫米雷达高度表和gps/北斗定位传感器对无人机高度进行检测，检测后的结果采用加权平均，即引入α值对这两种传感器的高度值进行加权平均处理，处理后采用kalman对高度值进行数据融合；根据ahrs航姿数据对高度值进行修正：
对于高度大于50m以上，采用gps/北斗定位传感器，将获取的高度数据直接进行kalman数据融合，然后，采用ahrs航姿数据进行高度修正；
其中h1是毫米雷达高度表采集的高度，h2是gps/北斗定位传感器采集的高度。
可选地，ahrs航姿数据进行高度修正具体为：
实施例5
本实施例对决策层进行详细说明，决策层通过如下步骤完成避障的：
p1,首先判断无人机与障碍物的相对距离，将相对距离进行三个部分的划分:小于n1m，n1m到n2m，n2m到n3m三个距离范围；
p2,距离划分完成后，根据无人机与障碍物的相对速度进行危险等级的划分:
当距离小于n1m，速度大于m1m/s,且预警时间小于qs，则属于危险等级，速度小于m1m/s时，属于警示等级；
当距离n1m≤r<n2m，速度大于m2m/s时,处于危险等级；当速度m1m/s≤v<m2m/s时，处于警示等级，当速度小于m1m/s时，处于提示等级；
当距离n2m≤r<n3m时，当速度大于m3m/s时,处于危险等级；当速度m2m/s≤v<m3m/s时，处于警示等级，当速度m1m/s≤v<m2m/s时，处于提示等级，当速度小于m1m/s时，处于不相关等级；
p3,然后判断无人机与地面的高度值，将该高度值h进行两个等级的划分；
当高度0m≤h<50m，根据毫米波防撞雷达传感器进行无人机与障碍物方位角的判断，主要进行障碍物在无人机飞行过程中得左边，右边，还是正前方。如果障碍物处于无人机的左边，则控制无人机进行右偏飞行，躲避障碍物；同理如果障碍物处于无人机的右边，控制无人机进行左偏飞行，躲避障碍物；如果障碍物处于无人机的正前方，则无人机爬升进行避障；
当高度h≥50m，根据毫米波防撞雷达进行无人机与障碍物方位角的判断，主要进行障碍物在无人机飞行过程中得左边，右边，还是正前方。如果障碍物处于无人机的左边，则控制无人机进行右偏飞行，躲避障碍物，同理如果障碍物处于无人机的右边，控制无人机进行左偏飞行，躲避障碍物；如果障碍物处于无人机飞行的正前方，由于此时无人机高度已经相对很高，对于这种情况采用紧急悬停后进行折返避障。
p4：对于危险等级，需要进行步骤p3的操作；对于警示等级，需要紧急减速后进行步骤p3的操作；对于提示等级和不相关等级，不用进行第三步骤的判断，返回重新进行检测。
产生控制指令后，向无人机主控制器发送控制命令，然后控制无人机舵机控制系统，控制无人机进行悬停折返、爬升、向右或向左偏飞行等避障动作，完成障碍。本申请给出了基于多传感器数据融合技术的单旋翼植保无人机避障系统实现原理以及给出多传感器融合技术方法；采用多传感器信息融合技术，能够更好地实现单旋翼植保无人机对植保环境的感知，获得更加准确的障碍物数据信息，做出更准确的避障决策以及做出避障控制还给出了数据预处理的流程以及给出相应的解决方案，为后续数据处理提供了更精准的数据信息。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。
- 还没有人留言评论。精彩留言会获得点赞！