代码收藏家技术教程 2025-02-12

四旋翼无人机建模（python）

前言

这篇博客主要是记录之前用python复现的一个四旋翼模型，起因是看了知乎的一篇文章，链接如下，作者用Matlab/Simulink 搭建四旋翼飞行器的动力学与运动学模型，当时正好在看全权老师的《多旋翼飞行器设计与控制》的建模部分，就结合起来用python复现了一下。代码仅供参考，具体的还是得看相关论文。

电机建模

四旋翼无人机使用的电机为无刷直流电机，而无刷直流电机模型可以等效为一个永磁直流电机模型，直接附上代码，这里涉及到三个参数，油门到电机稳态转速曲线斜率C_r，油门到电机稳态转速曲线零点w_b，单位为rad/s，电机相应时间常数T_m。

C_r=1148    
w_b=-141.4  
T_m=0.02    

#输入是油门指令throttle，输出是电机转速w
def function_w(throttle):
    w=(C_r*throttle+w_b)/(T_m+1)
    return w

四旋翼模型基本参数

c_T = 1.105*0.00001    # 螺旋桨拉力系数
c_M = 1.779*0.0000001  # 螺旋桨力矩系数
d = 0.225          # 机体中心和任一电机的距离(m)
m = 1.4            # 四旋翼飞行器质量(kg)
g = 9.8            # 重力加速度(m/s^2)
I_xx = 0.0211      # 四旋翼x轴转动惯量(kg·m^2)
I_yy = 0.0219      # 四旋翼y轴转动惯量(kg·m^2)
I_zz = 0.0366      # 四旋翼z轴转动惯量(kg·m^2)
J_RP = 0.0001287   # 整个电机转子和螺旋桨绕转轴的总转动惯量(kg·m^2)
Pos_z = -100       # 四旋翼初始高度

控制效率模型

这里参考的是全权老师书中的公式，多旋翼的飞行由多个螺旋桨驱动，螺旋桨转速决定多旋翼总拉力和力矩。

# 函数描述
# 1.作用：本函数用来计算螺旋桨旋转产生的总拉力和反扭力矩。
# 2.函数输入：
#   wi：四个螺旋桨的转速(rad/s)
#   c_T：螺旋桨拉力系数
#   c_M：螺旋桨力矩系数
#   d：机体中心和任一电机的距离(m)
# 3.函数输出：
#   f：螺旋桨拉力（机体轴）
#   tau_x：x轴反扭力矩（机体轴）
#   tau_y：y轴反扭力矩（机体轴）
#   tau_z：z轴反扭力矩（机体轴）
# 4.四旋翼构型为“X”型，螺旋桨序号如下所示：
#            3↓   1↑
#              \ /
#              / \
#            2↑   4↓
#   其中，↑表示螺旋桨逆时针旋转；↓表示螺旋桨顺时针旋转。

import math
import 四旋翼模型基本参数


def function_control_tau(w1,w2,w3,w4):
    f = 四旋翼模型基本参数.c_T * (w1**2 + w2**2 + w3**2 + w4**2)
    tau_x = 四旋翼模型基本参数.d * 四旋翼模型基本参数.c_T * (math.sqrt(2)/2) * (-w1**2 + w2**2 + w3**2 - w4**2)
    tau_y = 四旋翼模型基本参数.d * 四旋翼模型基本参数.c_T * (math.sqrt(2)/2) * (w1**2 - w2**2 + w3**2 - w4**2)
    tau_z = 四旋翼模型基本参数.c_M * (w1**2 + w2**2 - w3**2 - w4**2)
    return f,tau_x,tau_y,tau_z

姿态动力学模型

# 函数描述
# 1.作用：本函数为四旋翼的姿态动力学微分方程组，通过四旋翼所受力矩和螺旋
#         桨转速计算得到四旋翼的机体角速度
# 2.函数输入：
#   wi：四个螺旋桨的转速(rad/s)
#   tau_x：x轴反扭力矩（机体轴）(N·m)
#   tau_y：y轴反扭力矩（机体轴）(N·m)
#   tau_z：z轴反扭力矩（机体轴）(N·m)
#   I_xx：四旋翼x轴转动惯量(kg·m^2)
#   I_yy：四旋翼y轴转动惯量(kg·m^2)
#   I_zz：四旋翼z轴转动惯量(kg·m^2)
#   J_RP：整个电机转子和螺旋桨绕转轴的总转动惯量(kg·m^2)
#   p：四旋翼x轴角速度（机体轴)(rad/s)
#   q：四旋翼y轴角速度（机体轴)(rad/s)
#   r：四旋翼z轴角速度（机体轴)(rad/s)
# 3.函数输出：
#   p_dot：四旋翼x轴角加速度（机体轴)
#   q_dot：四旋翼y轴角加速度（机体轴)
#   r_dot：四旋翼z轴角加速度（机体轴)
# 4.四旋翼构型为“X”型，螺旋桨序号如下所示：
#            3↓   1↑
#              \ /
#              / \
#            2↑   4↓
#   其中，↑表示螺旋桨逆时针旋转；↓表示螺旋桨顺时针旋转。
import 四旋翼模型基本参数
import 控制效率模型
#初始时，机体轴下的四旋翼角速度为0

def function_attitude_first():
    p=0
    q=0
    r=0
    return  p,q,r

def function_attitude_rad(w1,w2,w3,w4,tau_x,tau_y,tau_z,p,q,r,t):
    Omega = -w1 + w2 - w3 + w4
    p_dot = (1/四旋翼模型基本参数.I_xx) * (tau_x + q * r * (四旋翼模型基本参数.I_yy - 四旋翼模型基本参数.I_zz) - 四旋翼模型基本参数.J_RP * q * Omega)
    q_dot = (1/四旋翼模型基本参数.I_yy) * (tau_y + p * r * (四旋翼模型基本参数.I_zz - 四旋翼模型基本参数.I_xx) + 四旋翼模型基本参数.J_RP * p * Omega)
    r_dot = (1/四旋翼模型基本参数.I_zz) * (tau_z + p * q * (四旋翼模型基本参数.I_xx - 四旋翼模型基本参数.I_yy) )
    return p_dot*t,q_dot*t,r_dot*t
#以上时三轴的角加速度，需要积分得到角速度,这里△t粗略当作1

位置动力学模型

# 函数描述
# 1.作用：本函数为四旋翼的位置动力学微分方程组，通过四旋翼所受拉力、姿态角
#         计算得到四旋翼飞行器的加速度
# 2.函数输入：
#   m：四旋翼飞行器质量(kg)
#   g：重力加速度(m/s^2)
#   phi：滚转角(rad)
#   theta：俯仰角(rad)
#   psi：偏航角(rad)
#   f：螺旋桨产生的总拉力(N)
# 3.函数输出：
#   v_x_dot：地球坐标系下沿x轴的速度的导数
#   v_y_dot：地球坐标系下沿y轴的速度的导数
#   v_z_dot：地球坐标系下沿z轴的速度的导数
# 4.四旋翼构型为“X”型，螺旋桨序号如下所示：
#            3↓   1↑
#              \ /
#              / \
#            2↑   4↓
#   其中，↑表示螺旋桨逆时针旋转；↓表示螺旋桨顺时针旋转。

import 四旋翼模型基本参数
import  math
import 控制效率模型

#初始时，滚转角，俯仰角，偏航角为0，之后迭代
def function_position_first():
    phi=0
    theta=0
    psi=0
    return phi,theta,psi

def function_position_v_dot(phi,theta,psi,f,t):
    v_x_dot = -f * (1/四旋翼模型基本参数.m) * (math.cos(psi) * math.sin(theta) * math.cos(phi) + math.sin(psi) * math.sin(phi))
    v_y_dot = -f * (1/四旋翼模型基本参数.m) * (math.sin(psi) * math.sin(theta) * math.cos(phi) - math.cos(psi) * math.sin(phi))
    v_z_dot = -四旋翼模型基本参数.g + f * (1/四旋翼模型基本参数.m) * math.cos(phi) * math.cos(theta)
    return v_x_dot*t,v_y_dot*t,v_z_dot*t
#以上所求实际为三轴的加速度，需要对时间积分得到速度，这里△t粗略当作1

四旋翼运动学模型

# 函数描述
# 1.作用：本函数为四旋翼的姿态运动学微分方程组，通过四旋翼的机体角速度
#         计算得到四旋翼的姿态角
# 2.函数输入：
#   p：四旋翼x轴角速度（机体轴)(rad/s)
#   q：四旋翼y轴角速度（机体轴)(rad/s)
#   r：四旋翼z轴角速度（机体轴)(rad/s)
#   phi：滚转角(rad)
#   theta：俯仰角(rad)
# 3.函数输出：
#   phi_dot：滚转角速度
#   theta_dot：俯仰角速度
#   psi_dot：偏航角速度
# 4.四旋翼构型为“X”型，螺旋桨序号如下所示：
#            3↓   1↑
#              \ /
#              / \
#            2↑   4↓
#   其中，↑表示螺旋桨逆时针旋转；↓表示螺旋桨顺时针旋转。
import math


def function_kinematics(p,q,r,phi,theta,psi,t):
    phi_dot = p + q * math.tan(theta) * math.sin(phi) + r * math.tan(theta) * math.cos(phi)
    theta_dot = q * math.cos(phi) - r * math.sin(phi)
    psi_dot = (q * math.sin(phi) + r * math.cos(phi)) / math.cos(theta)
    return phi_dot*t,theta_dot*t,psi_dot*t
#以上为三个姿态角的角速度，需要积分得到姿态角,这里△t粗略当作1

主函数

import 控制效率模型
import 位置动力学模型
import 姿态动力学模型
import 四旋翼运动学模型
import 电机模型
import matplotlib.pyplot as plt

t = int(input('请输入无人机仿真模拟的时间:'))  #设置飞行时间
throttle=float(input('请输入油门指令大小(为一个0到1的输入信号):'))
w=电机模型.function_w(throttle)

w1,w2,w3,w4=w,w,w,w  #输入螺旋桨的转速四个螺旋桨的转速
phi,theta,psi=位置动力学模型.function_position_first() #首次设置三个姿态角为0
p,q,r=姿态动力学模型.function_attitude_first()   #首次三轴角速度设置为0


#返回螺旋桨拉力和三轴的反扭力矩
f,tau_x,tau_y,tau_z=控制效率模型.function_control_tau(w1,w2,w3,w4)
v_x,v_y,v_z=位置动力学模型.function_position_v_dot(phi,theta,psi,f,t)
p,q,r=姿态动力学模型.function_attitude_rad(w1,w2,w3,w4,tau_x,tau_y,tau_z,p,q,r,t)
phi,theta,psi=四旋翼运动学模型.function_kinematics(p,q,r,phi,theta,psi,t)

print('位置:',v_x,v_y,v_z)


fig = plt.figure()
ax= fig.add_subplot(projection="3d")
x,y,z = [0,v_x],[0,v_y],[-100,v_z]
ax.scatter(x, y, z, c='blue', s=100)

ax.plot(x, y, z, color='black')
plt.show()