其他分享
首页 > 其他分享> > 四旋翼数学模型——位置模型

四旋翼数学模型——位置模型

作者:互联网


三、位置模型


力作用在物体上产生加速度,加速度积分得到速度,再积分得到位移。稍微容易令人迷惑的是,需要进行坐标变换。


3.1 符号说明

变量符号单位
四旋翼螺旋桨产生的合升力 T T T N \rm N N
地球系到机体系的旋转矩阵 R R R-
机体系 x y z xyz xyz 轴加速度 a x , a y , a z a_x, a_y, a_z ax​,ay​,az​ m / s 2 \rm m/s^2 m/s2
地球系 x y z xyz xyz 轴速度 v x , v y , v z v_x, v_y, v_z vx​,vy​,vz​ m / s \rm m/s m/s
地球系 x y z xyz xyz 轴位移 p x , p y , p z p_x, p_y, p_z px​,py​,pz​ m \rm m m
常量符号取值
无人机总质量 m m m 1.5 k g 1.5 \rm kg 1.5kg
重力加速度 g g g 9.8 m / s 2 9.8 \rm m/s^2 9.8m/s2

3.2 模型建立

首先受力分析

机体系 ( b b b 下标)采用 前右下 的坐标系,地球系( e e e下标)采用北东地坐标系。四旋翼共受两个力,机体系 − z -z −z 轴方向的升力 T T T,地球系 z z z 方向的重力 m g mg mg。

由于机体系不断变化,将升力转换在地球系下,将向量 [ 0 , 0 , − T ] [0, 0, -T] [0,0,−T] 从机体系转换到地球系,再与中立加速度相加,得到各轴上的合力为

[ F x F y F z ] = R [ 0 0 − T ] + m [ 0 0 g ] (3.1) \left [ \begin{array}{cc} F_x \\ F_y \\ F_z \end{array} \right ] = R \left [ \begin{array}{cc} 0 \\ 0 \\ -T \end{array} \right ] + m \left [ \begin{array}{cc} 0 \\ 0 \\ g \end{array} \right ] \tag{3.1} ⎣⎡​Fx​Fy​Fz​​⎦⎤​=R⎣⎡​00−T​⎦⎤​+m⎣⎡​00g​⎦⎤​(3.1)

其中, R R R 为机体系到地球系的旋转矩阵,使用欧拉角表示为

R = [ cos ⁡ θ cos ⁡ ψ cos ⁡ ψ sin ⁡ θ sin ⁡ ϕ − sin ⁡ ψ cos ⁡ ϕ cos ⁡ ψ sin ⁡ θ cos ⁡ ϕ + sin ⁡ ψ sin ⁡ ϕ cos ⁡ θ sin ⁡ ψ sin ⁡ ψ sin ⁡ θ sin ⁡ ϕ + cos ⁡ ψ cos ⁡ ϕ sin ⁡ ψ sin ⁡ θ cos ⁡ ϕ − cos ⁡ ψ sin ⁡ ϕ − sin ⁡ θ sin ⁡ ϕ cos ⁡ θ cos ⁡ ϕ cos ⁡ θ ] (3.2) R={ \left[ \begin{array}{ccc} \cos\theta \cos\psi& \cos\psi \sin\theta \sin\phi-\sin\psi \cos \phi & \cos\psi \sin\theta \cos\phi +\sin\psi \sin\phi \\ \cos\theta \sin\psi & \sin\psi \sin\theta \sin\phi+\cos\psi \cos\phi & \sin\psi \sin\theta \cos\phi-\cos\psi \sin\phi \\ -\sin\theta & \sin\phi \cos\theta & \cos\phi \cos\theta \end{array} \right ]} \tag{3.2} R=⎣⎡​cosθcosψcosθsinψ−sinθ​cosψsinθsinϕ−sinψcosϕsinψsinθsinϕ+cosψcosϕsinϕcosθ​cosψsinθcosϕ+sinψsinϕsinψsinθcosϕ−cosψsinϕcosϕcosθ​⎦⎤​(3.2)

ϕ , θ , ψ \phi, \theta, \psi ϕ,θ,ψ 分别为滚转角、俯仰角和偏航角,单位: r a d \rm rad rad。

将式(2.2)代入式(2.1) 得

{ F x = − T ( cos ⁡ ψ sin ⁡ θ cos ⁡ ϕ + sin ⁡ ψ sin ⁡ ϕ ) F y = − T ( sin ⁡ ψ sin ⁡ θ cos ⁡ ϕ − cos ⁡ ψ sin ⁡ ϕ ) F z = m g − T ( cos ⁡ ϕ cos ⁡ θ ) (3.3) \begin{cases} F_x = -T (\cos\psi \sin\theta \cos\phi +\sin\psi \sin\phi ) \\ F_y = -T (\sin\psi \sin\theta \cos\phi-\cos\psi \sin\phi) \\ F_z = mg-T(\cos\phi \cos\theta) \end{cases} \tag{3.3} ⎩⎪⎨⎪⎧​Fx​=−T(cosψsinθcosϕ+sinψsinϕ)Fy​=−T(sinψsinθcosϕ−cosψsinϕ)Fz​=mg−T(cosϕcosθ)​(3.3)

因此,地球系 x y z xyz xyz 方向得加速度为

v ˙ x = − T m ( cos ⁡ ψ sin ⁡ θ cos ⁡ ϕ + sin ⁡ ψ sin ⁡ ϕ ) v ˙ y = − T m ( sin ⁡ ψ sin ⁡ θ cos ⁡ ϕ − cos ⁡ ψ sin ⁡ ϕ ) v ˙ z = g − T m ( cos ⁡ ϕ cos ⁡ θ ) (3.4) \begin{aligned} &\dot v_x = -\frac{T}{m} (\cos\psi \sin\theta \cos\phi +\sin\psi \sin\phi ) \\ &\dot v_y = -\frac{T}{m} (\sin\psi \sin\theta \cos\phi-\cos\psi \sin\phi) \\ &\dot v_z = g-\frac{T}{m}(\cos\phi \cos\theta) \end{aligned} \tag{3.4} ​v˙x​=−mT​(cosψsinθcosϕ+sinψsinϕ)v˙y​=−mT​(sinψsinθcosϕ−cosψsinϕ)v˙z​=g−mT​(cosϕcosθ)​(3.4)

其中, v x , v y , v z v_x, v_y, v_z vx​,vy​,vz​ 为地球系各轴上的速度。速度是位移的导数,易得

p ˙ x = v x p ˙ y = v y p ˙ z = v z (3.5) \dot p_x = v_x \\ \dot p_y = v_y \\ \dot p_z = v_z \tag{3.5} p˙​x​=vx​p˙​y​=vy​p˙​z​=vz​(3.5)

其中, p x , p y , p z p_x, p_y, p_z px​,py​,pz​ 为地球系各轴上的位移。

式(3.4),(3.5) 即为最终的位置模型,描述了四旋翼位移与升力、姿态的关系。


3.3 计算与仿真

3.3.1 例1

设四旋翼质量为 1.5 k g 1.5\rm kg 1.5kg,重力加速度为 9.8 m / s 2 9.8 \rm m/s^2 9.8m/s2,四旋翼的滚转角、俯仰角和偏航角都是 0°。假设升力为 15 N 15\rm N 15N,计算 3 s 3 \rm s 3s 后四旋翼的速度与位移。

解:此时,旋转矩阵 R R R为单位阵,根据式(3.4),

v ˙ x = 0 v ˙ y = 0 v ˙ z = 9.8 − 15 / 1.5 = − 0.2 \begin{aligned} &\dot v_x =0 \\ &\dot v_y = 0 \\ &\dot v_z = 9.8- 15/1.5 =-0.2 \end{aligned} ​v˙x​=0v˙y​=0v˙z​=9.8−15/1.5=−0.2​

x , y , z x,y,z x,y,z 方向的加速度分别为 0 , 0 , − 0.2 0,0,-0.2 0,0,−0.2, 3 s 3\rm s 3s 后速度分别为 0 , 0 , − 0.6 m / s 0, 0, -0.6 \rm m/s 0,0,−0.6m/s,位移为

p x = 0 p y = 0 p z = 1 2 × ( − 0.2 ) × 3 2 = − 0.9 m \begin{aligned} & p_x =0 \\ & p_y = 0 \\ & p_z = \frac{1}{2} \times (-0.2) \times 3^2 = -0.9 \rm m \end{aligned} ​px​=0py​=0pz​=21​×(−0.2)×32=−0.9m​

地球系是北东地坐标系,因此无人机将升高 0.9 m 0. 9\rm m 0.9m。

3.3.2 例2

设四旋翼质量为 1.5 k g 1.5\rm kg 1.5kg,重力加速度为 9.8 m / s 2 9.8 \rm m/s^2 9.8m/s2,四旋翼的滚转角和俯仰角为10°,偏航角为 0°。假设升力为 15 N 15\rm N 15N,计算 3 s 3 \rm s 3s 后四旋翼的速度与位移。

phi = 10 / 180 * pi;   % 滚转角 (rad)
theta = 10 / 180 * pi; % 俯仰角 (rad)
psi = 0 / 180 * pi;   % 偏航角 (rad)

T = 15;     % 升力
m = 1.5;    % 质量
g = 9.8;    % 重力加速度

dt = 0.001;
a = zeros(3,1); 
v = zeros(3,1);
p = zeros(3,1);


for t=0:dt:3
    % 地球系各轴上加速度
    a(1) = -T/m * (cos(psi)*sin(theta)*cos(phi) + sin(psi)*sin(phi));
    a(2) = -T/m * (sin(psi)*sin(theta)*cos(phi) - cos(psi)*sin(phi));
    a(3) = g - T/m * (cos(psi) * cos(theta));
    % 矩形积分得到速度
    v(1) = v(1) + a(1) * dt;
    v(2) = v(2) + a(2) * dt;
    v(3) = v(3) + a(3) * dt;
    % 矩形积分得到位移
    p(1) = p(1) + v(1) * dt;
    p(2) = p(2) + v(2) * dt;
    p(3) = p(3) + v(3) * dt;
end

结果如下:
v = [ − 3.6849 3.6849 8.1895 ] v=\left [\begin{array}{cc} -3.6849 \\ 3.6849 \\ 8.1895 \end{array} \right ] v=⎣⎡​−3.68493.68498.1895​⎦⎤​

p = [ − 5.5310 5.5310 12.2925 ] p=\left [\begin{array}{cc} -5.5310 \\ 5.5310 \\ 12.2925 \end{array} \right ] p=⎣⎡​−5.53105.531012.2925​⎦⎤​

无人机俯仰角增大,无人机将向后飞( x x x轴负方向),因此 x x x 方向位移为负;滚转角增大,无人机向右飞( y y y轴正方向),因此 y y y 轴方向位移为正;由于无人机竖直方向拉力不足,无人机将向下掉落,实际中,如果不对高度进行控制,无人机倾斜时会出现掉高的现象。

3.2.3 例3

设四旋翼的滚转角和俯仰角接近 0°,偏航角为30°,当前机体系 x , z x,z x,z 方向速度为0, y y y 方向速度为 1 m / s 1 \rm m/s 1m/s,计算在四旋翼在地球系下速度。

解:从机体系速度转化到地球系速度的关系为

v e = R v b v_e = R v_b ve​=Rvb​

由于俯仰角和滚转角接近0°,旋转矩阵 R R R近似为

R = [ cos ⁡ ψ − sin ⁡ ψ 0 sin ⁡ ψ cos ⁡ ψ 0 0 0 1 ] (3.2) R={ \left[ \begin{array}{ccc} \cos\psi& -\sin\psi & 0 \\ \sin \psi & \cos\psi& 0 \\ 0 & 0& 1 \end{array} \right ]} \tag{3.2} R=⎣⎡​cosψsinψ0​−sinψcosψ0​001​⎦⎤​(3.2)

vb = [0; 1; 0];
psi = 45 / 180 * pi;
R = [cos(psi)  -sin(psi)   0;
    sin(psi)    cos(psi)   0;
    0           0          1];
ve = R * vb;

运行结果为

v e = [ − 0.7071 0.7071 0 ] v_e=\left [\begin{array}{cc} -0.7071 \\ 0.7071 \\ 0 \end{array} \right ] ve​=⎣⎡​−0.70710.70710​⎦⎤​

容易直观想象,偏航角为45°时,地球系 x , y x,y x,y 方向速度相等。

标签:cos,psi,数学模型,模型,phi,rm,theta,sin,旋翼
来源: https://blog.csdn.net/weixin_41869763/article/details/114371750