飞鸽传书--多旋翼由期望力求期望姿态

今天,讲解一个通过位置速度环求得期望力进而求得“多旋翼”的期望姿态的细分知识。之所以强调多旋翼,因为该方法是满足多旋翼(电机产生的合力与旋翼平面垂直)动力学方程,动力学分析是建模与控制的基础。

该方法的实现,可以简单的用以下几个公式来完成:
file
file

$$b_{1c}=(-sin\varphi,cos\varphi,0)^{T}\times (b_{3c})$$

其中f是期望力,而$R{c}$是期望旋转矩阵。

对该公式的理解,首先要了解一下以下基础知识:
file
该R方向余弦矩阵,表示从机体坐标系到地球固连坐标系的旋转矩阵,其满足以下性质:
file
无人机的姿态可以理解为欧拉角,四元数还有旋转矩阵。形象一点可以理解为空间的一个平面,再者可以理解为空间中的一个坐标系(三个轴xb,yb,zb)。

由位置速度环求得期望力与姿态平面垂直,也可以说与zb平行,故可将期望力矢量直接单位话作为zb,从而得到

$$b_{3c}$$

确定了期望姿态的一个轴。

为使机体xb与期望航向对齐,得到

$$b_{1c}=(-sin\varphi,cos\varphi,0)^{T}\times(b_{3c})$$

上式满足的条件是叉乘的两项不平行即 $\phi\ne-90^{o}$,可通过从机体坐标系到地球固连坐标系的旋转矩阵R的zb与 $ (-sin\varphi,cos\varphi,0)^{T} $做叉乘,即:$ (-sin\varphi,cos\varphi,0)^{T}\times(zb)=cos\phi(xb) $ ,故可证。

同时也可以

$${b_{1c}=(sin\varphi,-cos\varphi,0)^{T}\times(b_{3c})}$$

此时须满足 $\phi\ne90^{o}$

下面是向量叉乘的计算公式:
file
最后根据旋转矩阵的正交性质,可得

$$b_{2c}=b_{3c}\times(b_{1c}) $$

从而求得期望姿态矩阵。

当$ \phi=-90^{o} $时,此时理想拉力在XY平面,可将X轴竖直向下($ \theta=-90^{o} $,这样飞机是向前飞的效果保证安全),构建正交矩阵,从而得到

$$b_{1c}=(0,0,1)^{T} $$

pixhawk中的代码如下:

/* vector of desired yaw direction in XY plane, rotated by PI/2 */

math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f);

if (fabsf(body_z(2)) > SIGMA) {//SIGMA=0.000001f

/* desired body_x axis, orthogonal to body_z */

body_x = y_C % body_z;

/* keep nose to front while inverted upside down */

if (body_z(2) < 0.0f) {

//合力向上与向下,叉乘得到的X轴的正负不同,但须让头指向前方

body_x = -body_x;

}

body_x.normalize();

} else {

/* desired thrust is in XY plane, set X downside to construct correct matrix,

* but yaw component will not be used actually */

//在目标姿态Z轴水平时,由于欧拉角的奇异点(singularity)问题,忽略yaw误差;

body_x.zero();

body_x(2) = 1.0f;

}

/* desired body_y axis */

body_y = body_z % body_x;

求得期望姿态矩阵,进而求期望角速度及控制电机的作用量,可看我早些时候在CSDN写的博文,网址:https://blog.csdn.net/u014313096/article/details/79836919

更多无人机控制专业讲解,请关注微信公众号:欣飞鸽

开心飞翔,灵动如鸽,愿与无人机爱好者共创一片蓝天梦!

写作有误之处,欢迎指正。

一起奔跑,一起幻想,一起追梦,一起追求自由!