无人机线性控制器

无人机控制系统数学模型

本文推导来源

1. 控制系统数学模型

1.1 控制系统基本目标

该系统应能够根据无人机当前状态（位置、速度、加速度等信息）以及期望状态，通过油门控制量u.thrust和姿态控制量u.q来实现控制目标。

1.2 期望加速度的计算

期望加速度${\mathbf{a}}_{\text{des}}$的计算公式如下：
$${\mathbf{a}}_{\text{des}}={\mathbf{a}}_{\text{ref}}+{\mathbf{K}}_v({\mathbf{v}}_{\text{des}}-\mathbf{v})+{\mathbf{K}}_p({\mathbf{p}}_{\text{des}}-\mathbf{p})+\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]$$
其中：

• ${\mathbf{a}}_{\text{ref}}$：由轨迹规划提供的参考加速度
• ${\mathbf{K}}_v,{\mathbf{K}}_p$：分别为速度和位置的控制增益
• ${\mathbf{v}}_{\text{des}},{\mathbf{p}}_{\text{des}}$：表示期望速度和位置
• $\mathbf{v},\mathbf{p}$：表示当前速度和位置
• $g$：重力加速度，根据实际方向定义

1.3 推力计算（油门百分比）

基本公式

推力（以油门百分比表示）通过以下公式计算：
$$\text{throttle\_percentage}=\frac{a_{z,\text{des}}}{\text{thr2acc}}$$

参数说明

• $a_{z,\text{des}}$：z方向的期望加速度
• $\text{thr2acc}$：推力到加速度的映射系数，初始化为：
  $$\text{thr2acc}=\frac{g}{\text{hover\_percentage}}$$
  其中$\text{hover\_percentage}$为悬停油门百分比

悬停状态分析

在悬停状态下：

• 期望加速度：$a_{z,\text{des}}=g$
• 油门百分比：$\text{throttle\_percentage}=\text{hover\_percentage}$
  此时推力：$T=mg$，恰好能够抵消重力

1.4 姿态计算（滚转角和俯仰角）

滚转角$\varphi$和俯仰角$\theta$的计算采用小角度近似，公式如下：
$$\varphi =\frac{a_{x,\text{des}}\sin \psi -a_{y,\text{des}}\cos \psi }{g}$$
$$\theta =\frac{a_{x,\text{des}}\cos \psi +a_{y,\text{des}}\sin \psi }{g}$$
其中：

• $\psi$：表示当前偏航角
• $a_{x,\text{des}},a_{y,\text{des}}$：分别为x和y方向的期望加速度

1.5 悬停油门的处理

• 正常悬停：当位置和速度都为零时，$a_{z,\text{des}}=g$，此时推力$T=mg$，无人机能够实现悬停
• 特殊情况：当$a_{z,\text{des}}=0$时，推力为零，无人机处于自由落体状态。控制系统应确保无人机在需要下降时明确给出下降指令

通过控制系统在期望加速度中自动添加$+g$分量，可以确保无人机在默认状态下能够实现悬停

2. 推力模型的参数估计

2.1 估计目标

estimateThrustModel函数采用递归最小二乘（RLS）算法在线估计参数$\text{thr2acc}$，以应对系统参数的变化（如负载变化、电池衰减等）

2.2 数学模型

模型表达式为：
$$\text{est\_a}(2)=\text{thr2acc}\cdot T$$
其中：

• $\text{est\_a}(2)$：表示z方向的估计加速度
• $T$：为控制量，以推力表示

2.3 递归最小二乘（RLS）算法

RLS算法通过以下公式更新$\text{thr2acc}$：

• 增益计算：
  $$K_k=\frac{P_{k-1}\cdot T}{\lambda +T\cdot P_{k-1}\cdot T}$$
• 参数更新：
  $${\text{thr2acc}}_k={\text{thr2acc}}_{k-1}+K_k\cdot \left(\text{est\_a}(2)-T\cdot {\text{thr2acc}}_{k-1}\right)$$
• 协方差更新：
  $$P_k=\frac{1}{\lambda }\cdot \left(P_{k-1}-K_k\cdot T\cdot P_{k-1}\right)$$
  其中，$\lambda$为遗忘因子，由$\text{rho2\_}$参数控制，用于调节历史数据对估计结果的影响

2.4 数据选择逻辑

在数据选择方面，选取35ms到45ms之前的数据点，这样设计是为了匹配传感器和执行器之间的传输延迟。对于较旧的数据（时间差大于45ms）会被丢弃，而对于较新的数据（时间差小于35ms）也会被忽略

3. 总结

• 控制系统通过PD控制和加速度前馈控制，成功实现了对无人机位置和速度的有效控制
• 在推力计算方面，悬停状态通过$+g$加速度分量自动实现
• 递归最小二乘（RLS）算法在线估计了推力到加速度的映射关系，从而确保了控制的准确性和适应性