全自主巡航无人机项目思路：STM32/PX4 + ROS + AI 实现从传感融合到智能规 - IT评测·应用市场-qidao123.com

#include <math.h>
// 四元数结构体
typedef struct {
float w, x, y, z;
} Quaternion;
// 姿态估计参数
#define dt 0.01f // 采样周期
#define alpha 0.98f // 互补滤波系数
Quaternion attitude = {1.0f, 0.0f, 0.0f, 0.0f}; // 初始姿态
void attitudeUpdate(float acc[3], float gyro[3]) {
// 归一化加速度
float accMag = sqrt(acc[0]*acc[0] + acc[1]*acc[1] + acc[2]*acc[2]);
float ax = acc[0] / accMag;
float ay = acc[1] / accMag;
float az = acc[2] / accMag;
// 基于加速度计算俯仰角和横滚角
float pitch = atan2(-ax, sqrt(ay*ay + az*az));
float roll = atan2(ay, az);
// 构造基于加速度的四元数
Quaternion qAcc;
qAcc.w = cos(roll/2) * cos(pitch/2);
qAcc.x = cos(roll/2) * sin(pitch/2);
qAcc.y = sin(roll/2) * cos(pitch/2);
qAcc.z = -sin(roll/2) * sin(pitch/2);
// 基于陀螺仪数据的四元数微分方程
float qDot[4];
qDot[0] = 0.5f * (-attitude.x*gyro[0] - attitude.y*gyro[1] - attitude.z*gyro[2]);
qDot[1] = 0.5f * (attitude.w*gyro[0] + attitude.y*gyro[2] - attitude.z*gyro[1]);
qDot[2] = 0.5f * (attitude.w*gyro[1] - attitude.x*gyro[2] + attitude.z*gyro[0]);
qDot[3] = 0.5f * (attitude.w*gyro[2] + attitude.x*gyro[1] - attitude.y*gyro[0]);
// 更新姿态四元数
attitude.w += qDot[0] * dt;
attitude.x += qDot[1] * dt;
attitude.y += qDot[2] * dt;
attitude.z += qDot[3] * dt;
// 互补滤波
attitude.w = alpha * attitude.w + (1-alpha) * qAcc.w;
attitude.x = alpha * attitude.x + (1-alpha) * qAcc.x;
attitude.y = alpha * attitude.y + (1-alpha) * qAcc.y;
attitude.z = alpha * attitude.z + (1-alpha) * qAcc.z;
// 归一化四元数
float mag = sqrt(attitude.w*attitude.w + attitude.x*attitude.x +
attitude.y*attitude.y + attitude.z*attitude.z);
attitude.w /= mag;
attitude.x /= mag;
attitude.y /= mag;
attitude.z /= mag;
}

复制代码

#include <math.h>
typedef struct {
float Kp, Ki, Kd; // PID参数
float integral; // 积分项
float prevError; // 上一次的误差
} PIDController;
// 初始化PID控制器
void initPIDController(PIDController* pid, float Kp, float Ki, float Kd) {
pid->Kp = Kp;
pid->Ki = Ki;
pid->Kd = Kd;
pid->integral = 0.0f;
pid->prevError = 0.0f;
}
// PID控制器更新函数
float updatePID(PIDController* pid, float setpoint, float measurement, float dt) {
float error = setpoint - measurement;
// 比例项
float P = pid->Kp * error;
// 积分项
pid->integral += error * dt;
float I = pid->Ki * pid->integral;
// 微分项
float derivative = (error - pid->prevError) / dt;
float D = pid->Kd * derivative;
// 计算输出
float output = P + I + D;
// 更新上一次误差
pid->prevError = error;
return output;
}
// 位置控制主函数
void positionControl(float targetPosition[3], float currentPosition[3], float* outputs) {
static PIDController pidX, pidY, pidZ;
// 初始化PID控制器（仅在第一次调用时执行）
static int initialized = 0;
if (!initialized) {
initPIDController(&pidX, 1.0f, 0.1f, 0.05f); // 示例PID参数
initPIDController(&pidY, 1.0f, 0.1f, 0.05f);
initPIDController(&pidZ, 1.5f, 0.15f, 0.1f); // 垂直方向通常需要更强的控制
initialized = 1;
}
// 更新每个轴的PID控制器
float dt = 0.01f; // 假设控制周期为10ms
outputs[0] = updatePID(&pidX, targetPosition[0], currentPosition[0], dt);
outputs[1] = updatePID(&pidY, targetPosition[1], currentPosition[1], dt);
outputs[2] = updatePID(&pidZ, targetPosition[2], currentPosition[2], dt);
}

复制代码

import rospy
from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import OccupancyGrid, Path
import numpy as np
class AStarPlanner:
def __init__(self):
self.map = None
self.start = None
self.goal = None
self.path = []
# ROS节点初始化
rospy.init_node('astar_planner')
self.map_sub = rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
self.start_sub = rospy.Subscriber('/start_pose', PoseStamped, self.start_callback)
self.goal_sub = rospy.Subscriber('/goal_pose', PoseStamped, self.goal_callback)
self.path_pub = rospy.Publisher('/path', Path, queue_size=1)
def map_callback(self, msg):
self.map = np.array(msg.data).reshape((msg.info.height, msg.info.width))
def start_callback(self, msg):
self.start = (int(msg.pose.position.x), int(msg.pose.position.y))
self.plan()
def goal_callback(self, msg):
self.goal = (int(msg.pose.position.x), int(msg.pose.position.y))
self.plan()
def heuristic(self, a, b):
return np.sqrt((b[0] - a[0]) ** 2 + (b[1] - a[1]) ** 2)
def get_neighbors(self, node):
directions = [(0,1),(0,-1),(1,0),(-1,0),(1,1),(1,-1),(-1,1),(-1,-1)]
neighbors = []
for direction in directions:
neighbor = (node[0] + direction[0], node[1] + direction[1])
if 0 <= neighbor[0] < self.map.shape[0] and 0 <= neighbor[1] < self.map.shape[1]:
if self.map[neighbor] == 0: # 假设0表示自由空间
neighbors.append(neighbor)
return neighbors
def astar(self):
open_set = set([self.start])
closed_set = set()
came_from = {}
g_score = {self.start: 0}
f_score = {self.start: self.heuristic(self.start, self.goal)}
while open_set:
current = min(open_set, key=lambda x: f_score[x])
if current == self.goal:
path = []
while current in came_from:
path.append(current)
current = came_from[current]
path.append(self.start)
return path[::-1]
open_set.remove(current)
closed_set.add(current)
for neighbor in self.get_neighbors(current):
if neighbor in closed_set:
continue
tentative_g_score = g_score[current] + self.heuristic(current, neighbor)
if neighbor not in open_set:
open_set.add(neighbor)
elif tentative_g_score >= g_score[neighbor]:
continue
came_from[neighbor] = current
g_score[neighbor] = tentative_g_score
f_score[neighbor] = g_score[neighbor] + self.heuristic(neighbor, self.goal)
return None # 没有找到路径
def plan(self):
if self.map is not None and self.start is not None and self.goal is not None:
self.path = self.astar()
if self.path:
# 发布路径消息
path_msg = Path()
path_msg.header.frame_id = "map"
path_msg.header.stamp = rospy.Time.now()
for point in self.path:
pose = PoseStamped()
pose.pose.position.x = point[0]
pose.pose.position.y = point[1]
path_msg.poses.append(pose)
self.path_pub.publish(path_msg)
else:
rospy.logwarn("No path found")
if __name__ == '__main__':
planner = AStarPlanner()
rospy.spin()

复制代码