[发明专利]一种基于误差状态卡尔曼滤波的无人系统外力和外力矩在线估计方法

说明书

本发明属于无人机状态估计技术领域，具体涉及一种基于误差状态卡尔曼滤波的无人系统外力和外力矩在线估计方法。本发明提供的外力估计方法可以实时在线运行，运行频率和惯性测量单元一致，可以高达数百赫兹。本发明提供了一种基于误差状态卡尔曼滤波的四旋翼无人机外力和外力矩在线估计方法，能够在不依赖动作捕捉系统、RTK或者GPS等外部设备提供位置信息的前提下，只需要使用四旋翼无人机上的惯性测量单元、电机转速计、姿态反馈以及精确建模的四旋翼无人机动力学模型，即可进行外力和外力矩的同时估计。

技术领域

本发明属于无人机状态估计技术领域，涉及一种基于误差状态卡尔曼滤波的无人系统外力和外力矩在线估计方法，特别涉及一种基于误差状态卡尔曼滤波的四旋翼无人机外力和外力矩在线估计方法。

背景技术

近年来，由于四旋翼飞行器体积小，动作灵活，体积小，性能也越来越强，被用于执行各种各样的任务：其中它们要么需要与环境进行物理交互，如巡检和货物运输，要么需要与其他四旋翼飞行器近距离飞行，包括编队飞行和多机器人合作执行任务。在这些场景下，四旋翼无人机可能会经历较强的外力和外力矩，这些外力和外力矩很难进行建模，但会极大地影响四旋翼机的动态行为。准确估计外力和外力矩并对其作出适当的反应，对于四旋翼飞行器安全有效地完成给定的任务是至关重要的。

发明内容

有鉴于此，本发明提供了一种基于误差状态卡尔曼滤波的无人系统外力和外力矩在线估计方法，能够在不依赖动作捕捉系统、RTK或者GPS等外部设备提供位置信息的前提下，只需要使用四旋翼无人机上的惯性测量单元、电机转速计、姿态反馈以及精确建模的四旋翼动力学模型，借助误差状态卡尔曼滤波方法，实现在线实时地对四旋翼无人机收到的外力和外力矩进行准确估计。

实现本发明的技术方案如下：

一种基于误差状态卡尔曼滤波的无人系统外力和外力矩在线估计方法，该方法的步骤包括

步骤一，构建四旋翼无人机的系统动力学模型；

步骤二，构建四旋翼无人机的测量模型；

步骤三，根据步骤一构建的四旋翼无人机的系统动力学模型构建四旋翼无人机的真实状态动力学模型、标称状态动力学模型、误差状态动力学模型；

步骤四，采用显式欧拉法离散步骤三中构建的真实状态动力学模型，得到真实状态离散动力学方程，采用显式欧拉法离散步骤三中构建的标称状态动力学模型，得到标称状态离散动力学方程，采用显式欧拉法离散步骤三中构建的误差状态动力学模型，得到误差状态离散动力学方程；

步骤五，根据步骤四得到的真实状态离散动力学方程、标称状态离散动力学方程、误差状态离散动力学方程以及步骤二中构建的测量模型，求取雅可比，并将求取的雅可比代入到误差状态卡尔曼滤波公式中，得到状态估计递推式，并从得到的状态估计递推式中提取出外力和外力矩两项，完成四旋翼无人机外力和外力矩的在线估计。

所述的步骤一中，构建四旋翼无人机的系统动力学模型的方法为：