一种旋翼无人机自动校正导航控制系统的制作方法

本发明涉及一种导航系统，尤其一种旋翼无人机自动校正导航控制系统。

背景技术：

近年来，旋翼无人机以广泛应用于军事、紧急事件响应、监测、航拍和精密农业管理等领域，由于其不需要发射系统，能垂直起降，对起飞环境无要求，与固定翼无人机相比最大的优势是能够在空中自由悬停，而且飞行灵活性相当高超，可用各种速度，各种飞行剖面的航路进行飞行，因而比固定翼无人机在侦查、营救等艰难任务环境中，更具有应用优势。

现有技术中，导航模块提供稳定可靠的姿态、位置数据是旋翼无人机实现精准姿态控制和位置控制的前提，目前，应用的组合导航技术中，以惯性导航与卫星导航相结合的方式最为普遍。惯性测量单元动态响应特性良好，能够推算旋翼无人机的角度、速度、位置等信息，但长期工作会产生累积误差，全球定位系统能够直接给出载体的速度、位置信息，但更新频率很慢且速度位置信息的准确度易受遮挡干扰。

技术实现要素：

发明目的：针对上述问题，本发明的目的是提供一种旋翼无人机自动校正导航控制系统，避免产生累积误差以提供较为准确的姿态、位置数据。

技术方案：一种旋翼无人机自动校正导航控制系统，包括信号传输模块、自动导航模块、姿态控制模块、电机驱动模块，所述信号传输模块连接所述自动导航模块，所述自动导航模块连接所述姿态控制模块，所述姿态控制模块连接所述电机驱动模块，所述自动导航模块根据所述信号传输模块的信号指令进行导航姿态估算，所述姿态控制模块根据该姿态估算结果控制所述电机驱动模块驱动各旋翼完成指令动作，

所述自动导航模块包括惯性测量单元、延迟验证单元、自动校正单元、姿态估算单元，所述惯性测量单元连接所述姿态估算单元，所述姿态估算单元连接所述延迟验证单元，所述延迟验证单元连接所述自动校正单元，所述自动校正单元连接所述惯性测量单元，

所述惯性测量单元固联集成有三轴陀螺仪、三轴加速度计以检测角速度数据、加速度数据，并将该数据传递至所述姿态估算单元，

所述姿态估算单元对该数据进行解算处理，并估算得到t秒后的飞行姿态、位置、高度数据，同时将该估算数据传递至所述延迟验证单元，

所述延迟验证单元包括全球定位系统gnss、空速计tas、磁力计compass、气压计baro，所述延迟验证单元对t秒后的实际位置、高度、速度数据进行检测，并将检测结果与所述姿态估算单元在t秒前传递来的估算数据进行比对验证，然后将比对结果传递给所述自动校正单元，

所述自动校正单元根据比对结果将校正指令传递给所述惯性测量单元，当比对结果在设计的误差范围内，校正指令为无动作，当比对结果超出设计的误差范围外，所述惯性测量单元根据校正指令自动重设基准，并重新检测。

本发明的原理是：自动导航模块根据信号传输模块的信号指令进行导航姿态估算，姿态控制模块根据自动导航模块的姿态估算结果控制旋翼无人机的飞行姿态，电机驱动模块根据姿态控制模块的姿态控制指令驱动各旋翼完成指定动作。进行导航姿态估算时，姿态估算单元根据所述惯性测量单元的检测解算结果估算t秒后的飞行姿态、位置、高度等数据，并将该数据传输至姿态控制模块进行姿态控制，延迟验证单元在t秒后检测该时间点的实际位置、高度等数据，并与姿态估算单元在t秒前的估算结果进行比对验证，当估算数据与实际数据比对结果在设计的误差范围内时，姿态估算单元继续按照惯性测量单元的检测结果估算飞行姿态，并将估算结果继续传输至姿态控制模块。当姿态比对结果超出设计的误差范围外时，延迟验证单元输出误差指令到自动校正单元，自动校正单元根据误差指令调节惯性测量单元在现有基础上重新设定基准以实现自动校正，之后重新检测姿态数据，并将经过校正后的姿态数据重新发送姿态估算单元，姿态估算单元根据校正后的姿态数据重新估算，形成自动校正循环，避免导航系统产生累积误差，保证姿态数据的可靠性。同时，本方案的惯性测量单元采用与机体固联的三轴陀螺仪输出的角速度通过积分可以推算载体姿态且具有短时间内推算精度较高的特点，采用与机体固联的三轴加速度计通过各方向分量可以直接推算载体的姿态，具有较高的检测精度。全球定位系统gnss用于测量位置、高度信息，具有长期稳定性，可实现长期有效的精准定位；磁力计compass、气压计baro用于结合测量高度数据，以同时避免软磁与硬磁误差及风扰误差；空速计tas用于推算旋翼无人机相对于大气的运动速度。本方案利用传感器在频域上的互补特性进行姿态、位置数据融合，最终输出较为准确的姿态、位置等导航关键数据。

进一步，所述设计的误差范围为各旋翼姿态角度偏差±1°；位置偏差φ2cm；高度偏差3cm。

进一步，所述惯性测量单元采用mpu6000元件。本方案选用mpu6000元件作为惯性测量单元中，固联整合三轴陀螺仪、三轴加速度计进行旋翼无人机的姿态解算，可提升检测精度。

进一步，所述磁力计compass采用lsm303d元件，所述气压计baro采用ms5611元件，所述全球定位系统gnss采用neo-m8n元件，所述空速计tas采用mpxv7002元件。在利用传感器在频域上的互补特性进行姿态、位置数据融合后，可选用低成本的传感器实现高精度的测量数据。

进一步，所述姿态估算单元采用基于ekf算法构建的直接法与间接法相结合的组合导航模型。直接法所构建的系统模型一般具有强非线性，而采用间接法构建的模型非线性较弱，本方案采用直接法与间接法相结合的组合导航模型，提高组合导航的输出精度。

进一步，所述姿态控制模块包括姿态控制器、速度控制器、位置控制器、高度控制器。姿态控制器用于控制旋翼无人机的各旋翼的旋转姿态，速度控制器用于控制旋翼无人机的各旋翼的旋转速度，位置控制器用于控制旋翼无人机的行进及悬停位置，高度控制器用于控制旋翼无人机的行进及悬停高度。

进一步，还包括自动防撞模块，所述自动防撞模块的输入端与所述自动导航模块连接、输出端连接所述姿态控制模块，所述自动防撞模块包括超声波测距装置、减速悬停装置。自动导航模块估算t秒后飞行姿态等数据后，自动防撞模块通过超声波测距装置检测t秒后的姿态、位置等数据是否存在撞击威胁，如无撞击威胁则将自动导航模块的估算结果传输至姿态控制模块，如有撞击威胁则通过减速悬停装置传输悬停指令给姿态控制模块，并在电机驱动模块的控制下做减速悬停动作，以及时规避撞击威胁。

进一步，所述超声波测距装置采用hc-sr04超声波传感器。通过测量超声波高电平返回的时间，得到距离数据，再通过低通滤波进行数据平滑处理，得到稳定的距离信息反馈。

进一步，所述减速悬停装置采用光流传感器，以实现精准、稳定的定点悬停。

有益效果：与现有技术相比，本发明的优点是：利用传感器在频域上的互补特性进行姿态、位置数据融合和校正，最终输出较为准确的姿态、位置等导航关键信息，为组合导航提供准确的估算依据，避免累积误差对组合导航系统的影响。

附图说明

图1为本发明结构示意图。

具体实施方式

下面结合附图和具体实施例，进一步阐明本发明，这些实施例仅用于说明本发明而不用于限制本发明的范围。

一种旋翼无人机自动校正导航控制系统，如附图1所示，包括信号传输模块1、自动导航模块2、姿态控制模块3、电机驱动模块4、自动防撞模块5。

信号传输模块1连接自动导航模块2，自动导航模块2连接自动防撞模块5，自动防撞模块5连接姿态控制模块3，姿态控制模块3连接电机驱动模块4。

自动导航模块2包括惯性测量单元21、延迟验证单元22、自动校正单元23、姿态估算单元24，惯性测量单元21连接姿态估算单元24，姿态估算单元24连接延迟验证单元22，延迟验证单元22连接自动校正单元23，自动校正单元23连接惯性测量单元21。

惯性测量单元21采用mpu6000元件，固联集成有三轴陀螺仪211、三轴加速度计212。与机体固联的三轴陀螺仪输出的角速度通过积分可以推算载体姿态且具有短时间内推算精度较高的特点，与机体固联的三轴加速度计通过各方向分量可以直接推算载体的姿态，选用mpu6000元件作为惯性测量单元中，固联整合三轴陀螺仪、三轴加速度计进行旋翼无人机的姿态解算，提升检测精度。

延迟验证单元22包括全球定位系统gnss221、空速计tas222、磁力计compass223、气压计baro224。全球定位系统gnss用于测量位置、高度信息，具有长期稳定性，可实现长期有效的精准定位；磁力计compass、气压计baro用于结合测量高度数据，以同时避免软磁与硬磁误差及风扰误差；空速计tas用于推算的旋翼无人机相对于大气的运动速度。磁力计compass223采用lsm303d元件，气压计baro224采用ms5611元件，全球定位系统gnss221采用neo-m8n元件，空速计tas222采用mpxv7002元件。在利用传感器在频域上的互补特性进行姿态、位置数据融合后，可选用低成本的传感器实现高精度的测量数据。

姿态估算单元24采用基于ekf算法构建的直接法与间接法相结合的组合导航模型。直接法所构建的系统模型一般具有强非线性，而采用间接法构建的模型非线性较弱，本方案采用直接法与间接法相结合的组合导航模型，提高组合导航的输出精度。

自动防撞模块5包括超声波测距装置51、减速悬停装置52。

超声波测距装置51采用hc-sr04超声波传感器。通过测量超声波高电平返回的时间，得到距离数据，再通过低通滤波进行数据平滑处理，得到稳定的距离信息反馈。

减速悬停装置52采用光流传感器，以实现精准、稳定的定点悬停。

姿态控制模块3包括姿态控制器31、速度控制器32、位置控制器33、高度控制器34。姿态控制器用于控制旋翼无人机的各旋翼的旋转姿态，速度控制器用于控制旋翼无人机的各旋翼的旋转速度，位置控制器用于控制旋翼无人机的行进及悬停位置，高度控制器用于控制旋翼无人机的行进及悬停高度。

使用时，首先由自动导航模块根据信号传输模块的信号指令进行导航姿态估算：惯性测量单元结合三轴陀螺仪、三轴加速度计进行旋翼无人机的角速度、加速度解析，并将数据传输至姿态估算单元，由姿态估算单元建模、估算t秒后的旋翼无人机飞行姿态、位置、高度等导航信息，之后将该数据传输至自动防撞模块。

其次，自动防撞模块通过超声波测距装置检测t秒后的姿态、位置等数据是否存在撞击威胁，如无撞击威胁则将自动导航模块的估算结果传输至姿态控制模块，如有撞击威胁则通过减速悬停装置传输悬停指令给姿态控制模块，并在电机驱动模块的控制下做减速悬停动作，以及时规避撞击威胁。

再其次，自动防撞模块将无撞击威胁的姿态导航数据传输至姿态控制模块，并通过姿态控制器、速度控制器、位置控制器、高度控制器等进行旋翼无人机的导航控制。

最终，电机驱动模块根据姿态控制模块的姿态控制指令驱动各旋翼完成指定动作。

期间，在姿态控制模块按姿态导航数据实现导航控制的过程中，延迟验证单元在t秒后检测该时间点的实际位置、高度、速度等数据，并与姿态估算单元在t秒前的估算结果进行比对验证，当估算数据与实际数据比对结果在：各旋翼姿态角度偏差±1°；位置偏差φ2cm；高度偏差±3cm的范围内时，姿态估算单元继续按照惯性测量单元的检测结果估算飞行姿态，并将估算结果继续传输至姿态控制模块。当姿态比对结果超出：各角度偏差±1°；位置偏差φ2cm；高度偏差3cm的范围外时，延迟验证单元输出误差指令到自动校正单元，自动校正单元根据误差指令调节惯性测量单元的三轴陀螺仪、三轴加速度计在现有基础上重新设定基准以实现自动校正，之后重新检测姿态数据，并将经过校正后的姿态数据重新发送姿态估算单元，姿态估算单元根据校正后的姿态数据重新估算，形成自动校正循环，避免导航系统产生累积误差，保证姿态数据的可靠性。

本实施例利用传感器在频域上的互补特性进行姿态、位置数据融合和校正，最终输出较为准确的姿态、位置等导航关键信息，为组合导航提供准确的估算依据，避免累积误差对组合导航系统的影响。