四旋翼无人机姿态控制及坐标系转换和GPS数据解析

四旋翼无人机姿态控制及坐标系转换和GPS数据解析
引言
四旋翼无人机在农业、测绘、影视拍摄等领域发挥着重要作用。其控制精度直接关系到任务的执行效果。本文主要介绍四旋翼无人机的姿态控制,坐标系转换以及GPS数据的解析。

  1. 四旋翼无人机姿态控制
    四旋翼无人机的姿态通常用滚转角(roll)、俯仰角(pitch)和航向角(yaw)来表示。控制无人机的姿态,就是通过调整四个电机的转速,使无人机在空中达到期望的姿态。

1.1 姿态控制算法
常见的姿态控制算法有PID控制、LQR控制等。这里以PID控制为例介绍。

假设我们希望无人机达到一定的目标滚转角、目标俯仰角和目标航向角,我们可以通过PID控制器计算出四个电机应该提供的推力。
PID 控制算法原理:

  1. 比例控制 §:
    比例控制部分对误差进行线性缩放。它是误差与控制输出之间的比例因子。公式如下:

P_out = K_p * error

这里,K_p 是比例增益,error 是目标值和实际值之间的差。

  1. 积分控制 (I):
    积分控制用于消除系统的稳态误差。它通过累积过去的误差来工作。公式如下:

I_out = K_i * ∫(error)dt

这里,K_i 是积分增益。

  1. 微分控制 (D):
    微分控制预测误差的变化。它对误差的变化率进行控制,以减少过冲和震荡。公式如下:

D_out = K_d * d(error)/dt

这里,K_d 是微分增益。

PID 控制器的输出是这三个部分的总和:

PID_output = P_out + I_out + D_out

import time

class PIDController:
    def __init__(self, kp, ki, kd):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.previous_error = 0
        self.integral = 0
        self.last_time = time.time()

    def update(self, setpoint, measured_value):
        current_time = time.time()
        delta_time = current_time - self.last_time
        error = setpoint - measured_value
        
        # P
        p_out = self.kp * error
        
        # I
        self.integral += error * delta_time
        i_out = self.ki * self.integral
        
        # D
        derivative = (error - self.previous_error) / delta_time
        d_out = self.kd * derivative
        
        # Update state
        self.previous_error = error
        self.last_time = current_time
        
        return p_out + i_out + d_out

# 创建 PID 控制器实例
pid = PIDController(kp=1.0, ki=0.1, kd=0.01)

# 模拟无人机姿态调整过程
setpoint = 0  # 目标姿态角 (例如: 滚转角)
current_angle = 10  # 当前姿态角
while abs(setpoint - current_angle) > 0.01:
    # 使用PID控制器计算控制输出
    control_output = pid.update(setpoint, current_angle)
    # 使用控制输出调整无人机姿态
    # (此处仅为示例,具体调整方式需根据无人机硬件实现)
    current_angle +=

  1. 坐标系转换
    在无人机的控制中,常见的坐标系有惯性坐标系(Inertial Coordinate System)和机体坐标系(Body Coordinate System)。
    惯性坐标系通常是固定的,用于表示无人机在全局的位置和速度,而机体坐标系随无人机移动和旋转。要将一个在机体坐标系中的向量转换到惯性坐标系中,我们需要知道无人机的姿态,通常由欧拉角(Roll, Pitch, Yaw)表示。可以使用旋转矩阵来完成此转换。
    转换公式:
    如果我们有一个在机体坐标系中的三维向量 v_b = [x_b, y_b, z_b],我们可以使用以下公式将其转换到惯性坐标系 v_i = [x_i, y_i, z_i]:

v_i = R_z(yaw) * R_y(pitch) * R_x(roll) * v_b

这里,R_x, R_y, 和 R_z 是分别围绕x轴,y轴和z轴的旋转矩阵。

R_x(roll) = [[1, 0, 0], [0, cos(roll), -sin(roll)], [0, sin(roll), cos(roll)]]

R_y(pitch) = [[cos(pitch), 0, sin(pitch)], [0, 1, 0], [-sin(pitch), 0, cos(pitch)]]

R_z(yaw) = [[cos(yaw), -sin(yaw), 0], [sin(yaw), cos(yaw), 0], [0, 0, 1]]

import numpy as np

def rotation_matrix(roll, pitch, yaw):
    # Roll, pitch, yaw angles in radians
    # Create individual rotation matrices
    R_x = np.array([
        [1, 0, 0],
        [0, np.cos(roll), -np.sin(roll)],
        [0, np.sin(roll), np.cos(roll)]
    ])

    R_y = np.array([
        [np.cos(pitch), 0, np.sin(pitch)],
        [0, 1, 0],
        [-np.sin(pitch), 0, np.cos(pitch)]
    ])

    R_z = np.array([
        [np.cos(yaw), -np.sin(yaw), 0],
        [np.sin(yaw), np.cos(yaw), 0],
        [0, 0, 1]
    ])
    
    # Combine rotations
    R = np.dot(R_z, np.dot(R_y, R_x))
    return R

# Example usage:

# Roll, pitch, yaw in radians
roll = np.radians(30)
pitch = np.radians(20)
yaw = np.radians(10)

# Vector in the body coordinate system
v_b = np.array([1, 0, 0])

# Compute the rotation matrix
R = rotation_matrix(roll, pitch, yaw)

# Convert vector from the body coordinate system to the inertial coordinate system
v_i = np.dot(R, v_b)

print("Vector in inertial coordinate system:", v_i)

2.1 惯性坐标系和机体坐标系
惯性坐标系:通常是以地球为参考的坐标系,用于表示无人机在全局的位置和速度。
机体坐标系:是固定在无人机本身上的坐标系,用于表示无人机的姿态和角速度。
2.2 转换关系
利用欧拉角(roll, pitch, yaw),我们可以把在机体坐标系中的一个向量转换到惯性坐标系中,反之亦然。转换矩阵为旋转矩阵。

假设有一个在机体坐标系中的向量v_b,我们想要转换它到惯性坐标系,记作v_i。

v_i = R * v_b, 其中 R 为旋转矩阵。
四元数和欧拉角是两种描述物体在三维空间中旋转的方式。
四元数:
四元数是包含四个分量的复数,通常表示为 q = w + xi + yj + zk,其中 w 是实部,x, y, z 是虚部。四元数通常用于避免欧拉角的万向节锁问题。

欧拉角:
欧拉角使用三个角度来描述三维空间中的旋转,通常是滚动角(roll),俯仰角(pitch),和偏航角(yaw)。
四元数和欧拉角的关系:
我们可以将欧拉角转换为四元数,反之亦然。

欧拉角转四元数:
假设 roll,pitch 和 yaw 分别为 φ,θ 和 ψ,我们可以使用以下公式将它们转换为四元数:

w = cos(φ/2) * cos(θ/2) * cos(ψ/2) + sin(φ/2) * sin(θ/2) * sin(ψ/2)
x = sin(φ/2) * cos(θ/2) * cos(ψ/2) - cos(φ/2) * sin(θ/2) * sin(ψ/2)
y = cos(φ/2) * sin(θ/2) * cos(ψ/2) + sin(φ/2) * cos(θ/2) * sin(ψ/2)
z = cos(φ/2) * cos(θ/2) * sin(ψ/2) - sin(φ/2) * sin(θ/2) * cos(ψ/2)

四元数转欧拉角:
假设 q = w + xi + yj + zk 是一个四元数,我们可以使用以下公式将它转换为欧拉角:

roll (φ) = atan2(2*(w*x + y*z), 1 - 2*(x^2 + y^2))
pitch (θ) = asin(2*(w*y - z*x))
yaw (ψ) = atan2(2*(w*z + x*y), 1 - 2*(y^2 + z^2))

``
`

```cpp
import numpy as np

def euler_to_quaternion(roll, pitch, yaw):
    cr = np.cos(roll * 0.5)
    sr = np.sin(roll * 0.5)
    cp = np.cos(pitch * 0.5)
    sp = np.sin(pitch * 0.5)
    cy = np.cos(yaw * 0.5)
    sy = np.sin(yaw * 0.5)

    w = cr * cp * cy + sr * sp * sy
    x = sr * cp * cy - cr * sp * sy
    y = cr * sp * cy + sr * cp * sy
    z = cr * cp * sy - sr * sp * cy

    return w, x, y, z

def quaternion_to_euler(w, x, y, z):
    t0 = 2.0 * (w * x + y * z)
    t1 = 1.0 - 2.

欧拉角的万向节锁问题,可以通过想象一个坐在旋转椅上的人来理解。

让我们把这个人想象成一个小飞机。这个小飞机(或人)可以绕着三个轴旋转:绕着从头顶到脚的轴旋转叫做“滚动”(Roll),绕着从一只耳朵到另一只耳朵的轴旋转叫做“俯仰”(Pitch),绕着从胸口到背部的轴旋转叫做“偏航”(Yaw)。

现在,假设这个人首先把头向前低,差不多贴到胸口,这是极端的俯仰。然后,这个人试图向左或向右看(即尝试偏航)。你会发现,由于头已经贴在胸口,这个偏航动作实际上变成了滚动。此时,偏航和滚动变得混淆,我们失去了一个自由度的控制,这就是万向节锁。

在万向节锁的情况下,我们无法独立控制三个旋转,因为两个旋转混在一起了。在机器人学和航空领域,这是个大问题,因为你可能需要精确控制物体的方向。

四元数是一种避免万向节锁问题的数学工具,因为它不是通过三个角度来描述旋转,而是使用了四个数值,在数学上更稳定,不容易受到万向节锁问题的困扰。

  1. GPS数据解析
    GPS模块通常通过串口与无人机主控板连接,并通过NMEA 0183标准协议输出数据。

3.1 NMEA 0183数据格式
NMEA 0183数据包含多种句子,以" " 开头,如 " "开头,如" "开头,如"GPGGA"表示GPS固定数据。

3.2 Python代码示例
python
Copy code
import serial

打开串口

ser = serial.Serial(‘/dev/ttyS0’, 9600, timeout=1)

while True:
# 读取一行NMEA数据
line = ser.readline().decode()

# 解析GGA句子
if line.startswith('$GPGGA'):
    parts = line.split(',')
    latitude = float(parts[2])
    longitude = float(parts[4])
    print(f'Latitude: {latitude}, Longitude: {longitude}')

总结
本文介绍了四旋翼无人机的姿态控制,包括常见的PID控制算法。然后介绍了惯性坐标系与机体坐标系,并展示了如何进行坐标系转换。最后,我们讨论了如何使用Python解析GPS模块的NMEA 0183数据。这些知识对于无人机的控制和导航至关重要。

你可能感兴趣的:(机器人,无人机)