Var.
Ph.D. Candidate at Beihang University
首页
关于
文章分类
无人机位置环控制公式推导
2020-06-13 |Var. | 默认分类

令$\theta=pitch, \gamma=roll, \psi=yaw$,有

$$ \left[\begin{matrix} F_x\\F_y\\F_z \end{matrix}\right] = \boldsymbol{C}_b^n\left[\begin{matrix} 0\\0\\f \end{matrix}\right] = \left[\begin{matrix} \sin\gamma\cos\psi-\cos\gamma\sin\psi\sin\theta \\ -\sin\gamma\sin\psi-\cos\gamma\cos\psi\sin\theta \\ \cos\gamma\cos\theta \end{matrix}\right]f $$

其中$f$为无人机$b$系下的总拉力,$F_x,F_y,F_z$为世界坐标系下对应的期望力。

期望拉力

观察上式可得出,期望拉力

$$ f_d=\frac{F_z}{\cos\gamma\cos\theta} $$

期望俯仰角

对公式(1)进行稍微变换,$F_x\sin\psi+F_ycos\psi$,得

$$ \begin{aligned} F_x\sin\psi+F_ycos\psi&= f(\sin\gamma\cos\psi\sin\psi-\cos\gamma\sin\psi\sin\theta\sin\psi-\sin\gamma\sin\psi\cos\psi-\cos\gamma\cos\psi\sin\theta\cos\psi)\\&= -(\cos\gamma\sin\theta)f\\&= -(\cos\gamma\sin\theta)\frac{F_z}{\cos\gamma\cos\theta}\\&= -\tan\theta F_z \end{aligned} $$

期望俯仰角$\theta_d$为

$$ \theta_d=\arctan(-\frac{F_x\sin\psi+F_ycos\psi}{F_z}) $$

期望横滚角

对公式(1)进行稍微变换,$F_x\cos\psi-F_ysin\psi$,得

$$ \begin{aligned} F_x\cos\psi-F_ysin\psi&= f(\sin\gamma\cos\psi\cos\psi-\cos\gamma\sin\psi\sin\theta\cos\psi+\sin\gamma\sin\psi\sin\psi+\cos\gamma\cos\psi\sin\theta\sin\psi)\\&= (\sin\gamma)f\\&= \sin\gamma\frac{F_z}{\cos\gamma\cos\theta}\\&= \tan\gamma\frac{F_z}{\cos\theta} \end{aligned} $$

期望横滚角$\gamma_d$为

$$ \gamma_d=\arctan(\cos\theta\frac{F_x\cos\psi-F_ysin\psi}{F_z}) $$

respond-post-26

添加新评论

请填写称呼
请填写合法的电子邮箱地址
请填写合法的网站地址
请填写内容