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

2023-12-06 10:31:40

三、位置模型

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

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} ⎣⎡FxFyFz⎦⎤=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=vxp˙y=vyp˙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ψ0001⎦⎤(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 方向速度相等。

码农公寓