DDPG算法（深度确定性策略梯度）实现无人机三维空间避障导航

前言

在自主机器人和无人机系统中，避障是一个基础且关键的任务。尤其在复杂的三维环境中，无人机需要实时感知周围环境并做出决策，以安全高效地到达目标位置。本文将详细介绍如何使用深度确定性策略梯度（Deep Deterministic Policy Gradient, DDPG）算法来训练无人机在三维空间中自主避障导航。

DDPG是一种结合了DQN和策略梯度的强化学习算法，特别适合于具有连续状态和动作空间的控制问题。通过整合基于值函数和基于策略的学习方法，DDPG能够有效处理高维连续动作空间，这正是无人机控制所需的。

算法原理

DDPG算法简介

DDPG算法是一种用于连续动作空间的深度强化学习算法，结合了以下关键特性：

1. Actor-Critic架构：包含两个神经网络

   • Actor网络：生成确定性动作策略
   • Critic网络：评估动作价值

2. 确定性策略：与随机策略相比，确定性策略直接输出动作值，而非动作分布

3. 目标网络：使用目标Actor和Critic网络来提高训练稳定性

4. 经验回放：存储和重用过去的经验以打破样本相关性

5. 探索噪声：添加噪声到动作以实现探索

算法流程

1. 初始化Actor和Critic网络及其目标网络
2. 初始化经验回放缓冲区
3. 对于每个回合:
   • 获取初始状态
   • 对于每个时间步:
     • 根据当前策略加噪声选择动作
     • 执行动作并观察奖励和新状态
     • 存储转移到回放缓冲区
     • 从回放缓冲区采样随机小批量
     • 更新Critic网络
     • 更新Actor网络
     • 软更新目标网络

环境设置与代码实现

环境设置

%% 环境设置和参数定义
% 环境边界
env = struct(...
    'xMin', 0, ...
    'xMax', 100, ...
    'yMin', 0, ...
    'yMax', 100, ...
    'zMin', 0, ...
    'zMax', 50, ...
    'numObstacles', 30, ...
    'obstacleRadius', 5, ...
    'dt', 0.1 ... % 仿真时间步长
);

% 激光雷达参数
lidarParams = struct(...
    'numRays', 16, ...
    'maxRange', 20 ...
);

我们首先定义了一个100×100×50的三维环境，里面随机分布着30个半径为5的球形障碍物。无人机配备了16线激光雷达，最大探测距离为20个单位。

DDPG算法参数

% DDPG算法参数
params = struct(...
    'actorLearningRate', 0.0001, ...
    'criticLearningRate', 0.001, ...
    'gamma', 0.99, ... % 折扣因子
    'tau', 0.001, ... % 目标网络更新率
    'batchSize', 64, ...
    'bufferSize', 10000, ...
    'actionDim', 3, ... % 推力、俯仰、横滚
    'stateDim', 15 + lidarParams.numRays, ... % 3(位置) + 3(速度) + 3(姿态) + 3(目标) + 3(障碍物) + 激光雷达数据
    'maxEpisodes', 10000, ...
    'maxSteps', 200, ...
    'noiseStd', 0.3, ... % 探索噪声
    'minNoiseStd', 0.1, ...
    'noiseDecay', 0.99, ...
    'rewardScaling', 0.1 ...
);

这里定义了DDPG算法的关键参数：

• Actor和Critic网络的学习率
• 折扣因子gamma：决定未来奖励的重要性
• 目标网络更新率tau：控制目标网络更新的速度
• 经验回放缓冲区大小和采样批量大小
• 动作维度：3维（推力、俯仰、横滚）
• 状态维度：15 + 激光雷达射线数
• 探索噪声参数：初始标准差、最小标准差和衰减率

状态空间设计

function state = getState(droneState, goalPos, lidarDistances, obstacles, env, lidarParams)
    % 提取无人机位置、速度、姿态
    pos = droneState.pos;
    vel = droneState.vel;
    attitude = droneState.attitude;
    
    % 归一化位置和速度
    normalizedPos = (pos - [env.xMin, env.yMin, env.zMin]) ./ ...
        [env.xMax-env.xMin, env.yMax-env.yMin, env.zMax-env.zMin];
    normalizedGoal = (goalPos - [env.xMin, env.yMin, env.zMin]) ./ ...
        [env.xMax-env.xMin, env.yMax-env.yMin, env.zMax-env.zMin];
    normalizedVel = vel / 10; % 假设最大速度约为10 m/s
    
    % 寻找最近障碍物
    [minDist, closestObstacle] = findClosestObstacle(pos, obstacles);
    obstacleVector = (closestObstacle - pos) / minDist;
    
    % 归一化激光雷达读数
    normalizedLidar = lidarDistances / lidarParams.maxRange;
    
    % 组合所有状态
    state = [normalizedPos, normalizedVel, attitude, normalizedGoal, obstacleVector, normalizedLidar'];
end

无人机的状态空间包含以下组成部分：

1. 归一化的三维位置坐标
2. 归一化的三维速度向量
3. 三维姿态角（滚转、俯仰、偏航）
4. 归一化的目标位置
5. 到最近障碍物的向量
6. 归一化的激光雷达测距数据

这些状态组成了一个高维状态空间，共计15 + 激光雷达射线数维。

动作空间设计

无人机的动作空间是一个3维连续空间，包括：

1. 推力控制（垂直方向力）
2. 俯仰角控制（前后倾斜）
3. 横滚角控制（左右倾斜）

这三个控制量的范围均为[-1, 1]，然后映射到实际的物理控制范围。

奖励函数设计

function reward = getReward(droneState, goalPos, obstacles, env)
    % 提取无人机位置和速度
    pos = droneState.pos;
    vel = droneState.vel;
    
    % 目标奖励
    distToGoal = norm(pos - goalPos);
    maxPossibleDist = norm([env.xMax, env.yMax, env.zMax]);
    goalReward = -1 * (distToGoal / maxPossibleDist);
    
    % 成功奖励
    if distToGoal < 5
        goalReward = goalReward + 100;
    end
    
    % 进度奖励
    persistent prevDist
    if isempty(prevDist)
        prevDist = distToGoal;
    end
    progressReward = (prevDist - distToGoal) * 5;
    prevDist = distToGoal;
    
    % 避障奖励
    obstacleReward = 0;
    minObstacleDist = inf;
    for i = 1:size(obstacles, 1)
        dist = norm(pos - obstacles(i,:));
        minObstacleDist = min(minObstacleDist, dist);
        if dist < env.obstacleRadius * 2
            obstacleReward = obstacleReward - 2 * exp(-(dist/env.obstacleRadius));
        end
    end
    
    % 安全距离奖励
    if minObstacleDist > env.obstacleRadius * 3
        obstacleReward = obstacleReward + 0.5;
    end
    
    % 边界惩罚
    boundaryPenalty = 0;
    margin = 5;
    if pos(1) < env.xMin + margin || pos(1) > env.xMax - margin || ...
       pos(2) < env.yMin + margin || pos(2) > env.yMax - margin || ...
       pos(3) < env.zMin + margin || pos(3) > env.zMax - margin
        boundaryPenalty = -5;
    end
    
    % 速度惩罚（鼓励平滑轨迹）
    velocityPenalty = -0.1 * norm(vel)^2;
    
    % 能量效率奖励（惩罚高加速度）
    energyReward = -0.1 * norm(droneState.acc);
    
    % 姿态惩罚（鼓励稳定姿态）
    attitudePenalty = -0.1 * norm(droneState.attitude)^2;
    
    % 组合所有奖励
    reward = goalReward + progressReward + obstacleReward + boundaryPenalty + velocityPenalty + energyReward + attitudePenalty;
end

奖励函数是强化学习成功的关键，我们设计了多个奖励项来引导无人机的行为：

1. 目标奖励：与目标的距离越近，奖励越高
2. 成功奖励：当无人机接近目标时给予高额奖励
3. 进度奖励：鼓励无人机向目标靠近
4. 避障奖励：当无人机太靠近障碍物时给予负奖励
5. 安全距离奖励：鼓励无人机与障碍物保持安全距离
6. 边界惩罚：防止无人机飞出环境边界
7. 速度惩罚：鼓励平滑飞行，避免高速
8. 能量效率奖励：惩罚高加速度，鼓励节能飞行
9. 姿态惩罚：鼓励保持稳定姿态

激光雷达传感器模拟

function distances = simulateLiDAR(position, obstacles, env, lidarParams)
    % 模拟激光雷达传感器，向多个方向发射射线
    % 获取射线数量
    numRays = lidarParams.numRays;
    maxRange = lidarParams.maxRange;
    
    % 初始化距离数组
    distances = ones(numRays, 1) * maxRange;
    
    % 生成射线方向（在球面上均匀分布）
    [x, y, z] = sphere(sqrt(numRays)-1);
    directions = [x(:), y(:), z(:)];
    directions = directions(1:numRays, :);
    
    % 规范化方向
    for i = 1:numRays
        directions(i,:) = directions(i,:) / norm(directions(i,:));
    end
    
    % 发射射线并寻找与障碍物的交点
    for i = 1:numRays
        direction = directions(i,:);
        
        % 检查与障碍物的交点
        for j = 1:size(obstacles, 1)
            obstaclePos = obstacles(j,:);
            
            % 从射线原点到障碍物中心的向量
            toObstacle = obstaclePos - position;
            
            % 将此向量投影到射线方向上
            projection = dot(toObstacle, direction);
            
            % 如果障碍物在射线后方，跳过
            if projection < 0
                continue;
            end
            
            % 找到射线上离障碍物中心最近的点
            closestPoint = position + projection * direction;
            
            % 从最近点到障碍物中心的距离
            distToCenter = norm(closestPoint - obstaclePos);
            
            % 如果此点在障碍物半径内，我们有一个交点
            if distToCenter <= env.obstacleRadius
                % 计算实际交点（近似）
                % 从射线原点到交点的距离
                d = projection - sqrt(env.obstacleRadius^2 - distToCenter^2);
                
                % 如果此距离比之前的障碍物更近，则更新距离
                if d < distances(i)
                    distances(i) = d;
                end
            end
        end
        
        % 检查与环境边界的交点
        % ... (省略边界检测代码)
    end
end

激光雷达模拟是这个项目的一个关键部分，它使无人机能够"看见"周围的障碍物。我们在球面上均匀分布16条射线，每条射线检测从无人机到障碍物或环境边界的距离。

无人机动力学模型

function nextState = quadrotorDynamics(state, action, dt)
    % 四旋翼无人机动力学模型
    % 状态包含：位置、速度、加速度、姿态、角速度
    % 动作：推力、俯仰指令、横滚指令
    
    % 提取当前状态
    pos = state.pos;
    vel = state.vel;
    acc = state.acc;
    attitude = state.attitude; % [roll, pitch, yaw]
    angVel = state.angVel;
    
    % 将动作转换为推力和旋转命令
    thrust = (action(1) + 1) * 5; % 从[-1,1]缩放到[0,10]
    pitchCmd = action(2) * 0.3; % 从[-1,1]缩放到[-0.3,0.3]弧度
    rollCmd = action(3) * 0.3; % 从[-1,1]缩放到[-0.3,0.3]弧度
    
    % 简化的动力学（忽略复杂空气动力学）
    % 更新姿态
    targetAttitude = [rollCmd, pitchCmd, attitude(3)];
    attitudeRate = 2.0; % rad/s
    nextAttitude = attitude + attitudeRate * (targetAttitude - attitude) * dt;
    
    % 角速度
    nextAngVel = (targetAttitude - attitude) / dt;
    
    % 计算体坐标系中的加速度
    bodyAccel = [0, 0, thrust]; % z轴向上（体坐标系）
    
    % 从体坐标系到世界坐标系的旋转矩阵
    R = bodyToWorldRotation(nextAttitude);
    
    % 世界坐标系中的加速度
    gravity = [0, 0, -9.81];
    worldAccel = R * bodyAccel' + gravity';
    
    % 更新速度和位置
    nextAcc = worldAccel';
    nextVel = vel + nextAcc * dt;
    nextPos = pos + nextVel * dt;
    
    % 组合为下一状态
    nextState = struct('pos', nextPos, 'vel', nextVel, 'acc', nextAcc, ...
                      'attitude', nextAttitude, 'angVel', nextAngVel);
end

这个函数实现了简化的四旋翼无人机动力学模型，将控制动作转换为无人机的下一个物理状态。主要步骤包括：

1. 将归一化动作映射到实际控制量（推力和姿态角）
2. 根据控制命令更新目标姿态
3. 计算体坐标系中的加速度
4. 将加速度转换到世界坐标系
5. 积分更新速度和位置

神经网络实现

function network = initializeNetwork(inputSize, layerSizes, activationFunctions)
    % 初始化神经网络
    
    network = struct();
    network.layerSizes = [inputSize, layerSizes];
    network.numLayers = length(network.layerSizes) - 1;
    network.activationFunctions = activationFunctions;

    network.weights = cell(network.numLayers, 1);
    network.biases = cell(network.numLayers, 1);
    
    for i = 1:network.numLayers
        inputSize = network.layerSizes(i);
        outputSize = network.layerSizes(i+1);
        
        % 使用何等初始化
        bound = sqrt(6 / (inputSize + outputSize));
        network.weights{i} = rand(outputSize, inputSize) * 2 * bound - bound;
        
        % 初始化偏置为0
        network.biases{i} = zeros(outputSize, 1);
    end
end

我们使用MATLAB实现了基本的前馈神经网络，包括：

• 多层结构
• 支持不同的激活函数（ReLU、tanh、sigmoid等）
• 何等初始化（He initialization）
• 前向传播和反向传播

Actor网络使用的结构是：

• 输入层：状态维度
• 隐藏层1：256个神经元，ReLU激活
• 隐藏层2：128个神经元，ReLU激活
• 输出层：3个神经元（动作维度），tanh激活

Critic网络使用的结构是：

• 输入层：状态维度+动作维度
• 隐藏层1：256个神经元，ReLU激活
• 隐藏层2：128个神经元，ReLU激活
• 输出层：1个神经元（Q值）

训练循环

%% 训练循环
totalReward = zeros(params.maxEpisodes, 1);
episodeLength = zeros(params.maxEpisodes, 1);
bestTrajectory = [];
bestReward = -inf;

fprintf('开始训练 DDPG 算法...\n');

for episode = 1:params.maxEpisodes
    % 重置无人机状态
    droneState = initialDroneState;
    
    % 获取激光雷达数据
    lidarDistances = simulateLiDAR(droneState.pos, obstacles, env, lidarParams);
    
    % 构建RL代理的状态
    state = getState(droneState, goalPos, lidarDistances, obstacles, env, lidarParams);
    
    episodeReward = 0;
    trajectory = zeros(params.maxSteps, 3); % 存储轨迹用于可视化
    
    % 初始化探索噪声
    noise = params.noiseStd * randn(1, params.actionDim);
    
    for step = 1:params.maxSteps
        % 从Actor网络获取动作
        action = forward(actorNet, state);
        
        % 应用噪声和裁剪
        actionWithNoise = action(:)' + noise; % 转为行向量，加噪声
        actionClipped = max(min(actionWithNoise, 1), -1); % 裁剪动作
        
        % 应用无人机动力学模型获取下一状态
        nextDroneState = quadrotorDynamics(droneState, actionClipped, env.dt);
        
        % 获取新位置的激光雷达数据
        nextLidarDistances = simulateLiDAR(nextDroneState.pos, obstacles, env, lidarParams);
        
        % 构建下一个状态
        nextState = getState(nextDroneState, goalPos, nextLidarDistances, obstacles, env, lidarParams);
        
        % 计算奖励
        reward = getReward(nextDroneState, goalPos, obstacles, env) * params.rewardScaling;
        
        % 检查终止条件
        done = (norm(nextDroneState.pos - goalPos) < 5) || checkCollision(nextDroneState.pos, obstacles, env);
        
        % 存储轨迹用于可视化
        trajectory(step,:) = nextDroneState.pos;
        
        % 将经验存储到回放缓冲区
        experience = struct(...
            'state', state, ...
            'action', actionClipped, ...
            'reward', reward, ...
            'nextState', nextState, ...
            'done', done ...
        );
        
        bufferCount = mod(bufferCount, params.bufferSize) + 1;
        replayBuffer{bufferCount} = experience;
        if bufferCount == params.bufferSize
            bufferIsFull = true;
        end
        
        % 如果有足够样本则训练网络
        if bufferIsFull || bufferCount >= params.batchSize
            % 从回放缓冲区采样小批量
            if bufferIsFull
                indices = randi(params.bufferSize, params.batchSize, 1);
            else
                indices = randi(bufferCount, params.batchSize, 1);
            end
            
            batch = replayBuffer(indices);
            
            % 准备批量数据
            batchStates = zeros(params.stateDim, params.batchSize);
            batchActions = zeros(params.actionDim, params.batchSize);
            batchRewards = zeros(1, params.batchSize);
            batchNextStates = zeros(params.stateDim, params.batchSize);
            batchDones = zeros(1, params.batchSize);
            
            % 填充批量数据
            
            % 计算目标Q值
            targetActions = forward(targetActorNet, batchNextStates);
            targetInputs = [batchNextStates; targetActions];
            targetQ = forward(targetCriticNet, targetInputs);
            
            targets = batchRewards + params.gamma * targetQ .* (1 - batchDones);
            
            % 更新Critic网络
            criticInputs = [batchStates; batchActions];
            criticNet = updateNetwork(criticNet, criticInputs, targets, params.criticLearningRate);
            
            % 使用策略梯度更新Actor网络
            predictedActions = forward(actorNet, batchStates);
            qInputs = [batchStates; predictedActions];
            qValues = forward(criticNet, qInputs);
            
            actorNet = updatePolicyGradient(actorNet, batchStates, predictedActions, qValues, params.actorLearningRate);
            
            % 软更新目标网络
            targetActorNet = softUpdateNetwork(targetActorNet, actorNet, params.tau);
            targetCriticNet = softUpdateNetwork(targetCriticNet, criticNet, params.tau);
        end
        
        % 更新状态
        state = nextState;
        droneState = nextDroneState;
        episodeReward = episodeReward + reward;
        
        % 检查是否完成
        if done
            % 截断轨迹到实际步数
            trajectory = trajectory(1:step,:);
            
            % 保存最佳轨迹
            if episodeReward > bestReward && norm(droneState.pos - goalPos) < 5
                bestReward = episodeReward;
                bestTrajectory = trajectory;
            end
            
            break;
        end
        
        % 衰减探索噪声
        noise = noise * 0.99;
    end
    
    % 衰减下一轮探索噪声
    params.noiseStd = max(params.minNoiseStd, params.noiseStd * params.noiseDecay);
    
    % 定期绘制训练进度与轨迹可视化
    % ...
end

训练循环实现了DDPG算法的完整流程，包括：

1. 环境重置
2. 与环境交互收集经验
3. 存储经验到回放缓冲区
4. 从缓冲区采样小批量数据
5. 更新Critic和Actor网络
6. 软更新目标网络
7. 探索噪声衰减
8. 训练进度可视化

可视化与结果分析

轨迹可视化

function visualizeTrajectory(trajectory, obstacles, env)
    % 在3D空间中可视化带障碍物的优化轨迹
    
    figure('Position', [100, 100, 1000, 800]);
    
    % 绘制环境边界
    plot3([env.xMin, env.xMax, env.xMax, env.xMin, env.xMin], ...
          [env.yMin, env.yMin, env.yMax, env.yMax, env.yMin], ...
          [env.zMin, env.zMin, env.zMin, env.zMin, env.zMin], 'k-', 'LineWidth', 2);
    hold on;
    % ... 绘制其他边界
    
    % 绘制障碍物
    for i = 1:size(obstacles, 1)
        [X, Y, Z] = sphere(20);
        X = X * env.obstacleRadius + obstacles(i,1);
        Y = Y * env.obstacleRadius + obstacles(i,2);
        Z = Z * env.obstacleRadius + obstacles(i,3);
        surf(X, Y, Z, 'FaceColor', [0.7, 0.7, 0.7], 'EdgeColor', 'none', 'FaceAlpha', 0.5);
    end
    
    % 绘制轨迹
    plot3(trajectory(:,1), trajectory(:,2), trajectory(:,3), 'r-', 'LineWidth', 2);
    plot3(trajectory(1,1), trajectory(1,2), trajectory(1,3), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g');
    plot3(trajectory(end,1), trajectory(end,2), trajectory(end,3), 'bo', 'MarkerSize', 10, 'MarkerFaceColor', 'b');
    
    % 在轨迹上的各个点绘制无人机
    numPoints = size(trajectory, 1);
    step = max(1, floor(numPoints / 10));
    for i = 1:step:numPoints
        drawDrone(trajectory(i,:), 1.0);
    end
    
    % 设置标签和标题
    xlabel('X');
    ylabel('Y');
    zlabel('Z');
    title('无人机轨迹与障碍物');
    grid on;
    axis equal;
    view(30, 30);
end

可视化函数生成了三维空间中的轨迹，包括环境边界、障碍物和无人机的位置。这有助于直观理解训练结果。

激光雷达可视化

function visualizeTrajectoryWithLidar(trajectory, lidarReadings, obstacles, env, lidarParams)
    
    
    % 生成激光雷达方向
    [x, y, z] = sphere(sqrt(lidarParams.numRays)-1);
    directions = [x(:), y(:), z(:)];
    directions = directions(1:lidarParams.numRays, :);
    
    % 规范化方向
    for i = 1:lidarParams.numRays
        directions(i,:) = directions(i,:) / norm(directions(i,:));
    end
    
    % 在选定点显示激光雷达射线
    numPoints = size(trajectory, 1);
    lidarPoints = round(linspace(1, numPoints, min(10, numPoints)));
    
    for i = lidarPoints
        pos = trajectory(i,:);
        
        % 绘制无人机
        drawDrone(pos, 1.0);
        
        % 绘制激光雷达射线
        for j = 1:lidarParams.numRays
            lidarDist = lidarReadings(i,j);
            if lidarDist < lidarParams.maxRange
                endpoint = pos + directions(j,:) * lidarDist;
                plot3([pos(1), endpoint(1)], [pos(2), endpoint(2)], [pos(3), endpoint(3)], 'g-', 'LineWidth', 0.5);
                plot3(endpoint(1), endpoint(2), endpoint(3), 'g.', 'MarkerSize', 5);
            end
        end
    end
    
    
end

这个函数在轨迹可视化的基础上增加了激光雷达射线的显示，帮助理解无人机是如何感知和避开障碍物的。

训练与测试结果

完成训练后，算法能够生成一条从起点到终点的平滑轨迹，同时有效避开所有障碍物。无人机学会了以下行为：

1. 向目标点导航
2. 提前探测并绕开障碍物
3. 保持平滑的飞行轨迹
4. 避免大幅度姿态变化
5. 保持安全的飞行高度

我们还使用训练好的策略进行了测试：

%% 使用训练好的策略运行测试
fprintf('\n运行测试轮次（使用训练好的策略）...\n');
droneState = initialDroneState;
lidarDistances = simulateLiDAR(droneState.pos, obstacles, env, lidarParams);
state = getState(droneState, goalPos, lidarDistances, obstacles, env, lidarParams);

testTrajectory = zeros(params.maxSteps, 3);
lidarReadings = zeros(params.maxSteps, lidarParams.numRays);

for step = 1:params.maxSteps
    % 从训练好的Actor获取动作（无噪声）
    action = forward(actorNet, state');
    action = action';
    action = max(min(action, 1), -1);
    
    % 应用无人机动力学
    nextDroneState = quadrotorDynamics(droneState, action, env.dt);
    
    % 获取激光雷达数据
    nextLidarDistances = simulateLiDAR(nextDroneState.pos, obstacles, env, lidarParams);
    
    % 更新状态
    nextState = getState(nextDroneState, goalPos, nextLidarDistances, obstacles, env, lidarParams);
    
    % 记录位置和激光雷达数据
    testTrajectory(step,:) = nextDroneState.pos;
    lidarReadings(step,:) = nextLidarDistances';
    
    % ... 其他代码
end

% 可视化测试轨迹和激光雷达数据
visualizeTrajectoryWithLidar(testTrajectory, lidarReadings, obstacles, env, lidarParams);
title('训练后策略的测试轨迹（含激光雷达）');

测试结果表明，训练好的策略能够在没有探索噪声的情况下稳定导航，有效避开障碍物并到达目标位置。

结论

本文详细介绍了如何使用DDPG算法实现无人机在三维空间中的自主避障导航。实现了

1. 设计了适合无人机控制的状态空间和动作空间
2. 实现了高效的奖励函数，平衡了多个飞行目标
3. 模拟了激光雷达传感器，使无人机能够感知周围环境
4. 训练出能够有效避障的控制策略

在连续控制问题上展现出了强大的能力，为无人机自主导航提供了一种有效的解决方案。