matlab仿真四自由度机械臂运动规划，RRT

这个代码是从github上clone的，共享一下，我稍微改了一点也不知道对不对，欢迎大家讨论。

他在RRT扩展到终点后，没有回溯，而用了k邻近和建图搜索最短路径，不知道这是什么缝合版RRT，有学术的叫法吗？

github原库：https://github.com/josejosepht/4-DOF-2-link-arm-motion-planning

我改的：https://download.csdn.net/download/qq_46217688/88034897

主要贴出来两个文件吧。

RRT.m

% Input: robot -> A 4-DOF robot encoded as a SerialLink object
%        q_min -> 1x4 vector of minimum angle for each joint
%        q_max -> 1x4 vector of maximum angle for each joint
%        q_start -> 1x4 vector denoting the start configuration
%        q_goal -> 1x4 vector denoting the goal configuration
%        link_radius -> Scalar denoting radius of each robot link's
%                       cylindrical body
%        sphere_centers -> Nx3 matrix containing the centers of N spherical
%                          obstacles
%        sphere_radii -> Nx1 vector containing the radii of N spherical
%                        obstacles
% Output: path -> Nx4 matrix containing a collision-free path between
%                 q_start and q_goal, if a path is found. The first row
%                 should be q_start, the final row should be q_goal.
%         path_found -> Boolean denoting whether a path was found

function [path, path_found] = RRTConnect(robot, q_min, q_max, q_start, q_goal, link_radius, sphere_centers, sphere_radii)
    % Define the configuration space
    q_limits = [q_min;q_max];
    
    % Initialize the tree with the start configuration as the root node
    start_node = q_start;
    tree = start_node;
    
    % Define the maximum number of iterations
    max_iterations = 10000;
    
    % Define the step size for extending the tree
    step_size = 0.1;
    
    % Define the goal region
    goal_region = 0.1;
    
    % Initialize the flag for reaching the goal configuration
    path_found = false;
    count = 1;
    % RRT algorithm
    for i = 1:max_iterations
        
        
        % Generate a random configuration
        q_rand = q_limits(1,:) + (q_limits(2,:) - q_limits(1,:)).*rand(1,4);
        robot.plot(q_rand)
        % Find the nearest node in the tree
        dist = vecnorm(tree(:,1:4) - q_rand, 2, 2);
        [min_dist, nearest_node] = min(dist);
        q_near = tree(nearest_node,1:4);
        
        % Extend the tree towards the random configuration
        delta_q = step_size * (q_rand - q_near) / min_dist;
        q_new = q_near + delta_q;
        robot.plot(q_new)
    
        % Check if the new configuration is valid and collision-free
        if ~check_edge(robot,q_near,q_new, link_radius, sphere_centers, sphere_radii)
            % Add the new configuration to the tree as a new node
            new_node = q_new;
            tree = [tree; new_node];
            count = count +1;
            fprintf('the size of tree is: %d\n', count);
    
            % Check if the goal configuration is reached
            if norm(q_new - q_goal) <= goal_region
                path_found = true;
                tree = [tree; q_goal];
                break;
            end
        end
    end
    
    kdTree= KDTreeSearcher(tree);
    adjMatrix = zeros(length(tree));
    
    num_neighbors = 10;
    fprintf('Computing the adjmatrix ...\n');
    % Compute the edges between samples using the k-nearest neighbors algorithm
    for i = 1:length(tree)
    % Find the k-nearest neighbors of sample i
    [idx, dist] = knnsearch(kdTree, tree(i,:), 'K', num_neighbors+1);
    % For each neighbor, check if the edge is collision-free
        for j = 2:num_neighbors+1
            if ~check_edge(robot, tree(i,:), tree(idx(j),:), link_radius, sphere_centers, sphere_radii)
            % Add the edge to the adjacency matrix
            adjMatrix(i,idx(j)) = dist(j);
            adjMatrix(idx(j),i) = dist(j);
            end
        end
    end
    adjacency = adjMatrix;
    fprintf('finished the compution\n');

%     [idx,~]=knnsearch(tree,[q_start;q_goal],K=1);
    G = graph(adjacency);
    path_idx = shortestpath(G,1,length(tree));
    n = length(path_idx);
    path=zeros(n,4);
    for i=1:length(path_idx)
         path(i,:) = tree(path_idx(i),:);
    end
%     path_idx = shortestpath(G,idx(1),idx(2));  

%     n = length(path_idx)+2;
%     path=zeros(n,4);
%     for i=1:length(path_idx)
%         path(i+1,:) = tree(path_idx(i),:);
%     end
%     path(1,:)=q_start;
%     path(end,:) = q_goal;
end

这里RRT最后没有回溯，当然他过程中就没有建立父节点存放上下关系，也回溯不了。

最后几行稍微改了一下。

SmoothPath.m

function smoothed_path_new = SmoothenPath(robot, path, link_radius, sphere_centers, sphere_radii)
smoothed_path=path;
smoothed_path_new = path;
if smoothed_path ~= smoothed_path_new
    smoothed_path = smoothed_path_new;
    for i=1:length(smoothed_path)-2
        j = i+2;
        if ~check_edge(robot, smoothed_path(i,:), smoothed_path(j,:), link_radius, sphere_centers, sphere_radii,25)
            smoothed_path_new = [smoothed_path(1:i,:);smoothed_path(j:end,:)]
        end
    end
end
% for i=1:length(smoothed_path)
%     q = smoothed_path(i,:);
%     if q==path(end,:)
%         break
%     end
%     for j=[length(smoothed_path):-1:1]
%         if i<j
%             if ~check_edge(robot, smoothed_path(i,:), smoothed_path(j,:), link_radius, sphere_centers, sphere_radii,25)
%                 smoothed_path=[smoothed_path(1:i,:);smoothed_path(j:end,:)]
%             end
%         end
%     end
% end
% end

这里被注释的是原来的代码，有报错，我改成了每次判断i和i+2之间的点是否碰撞，不碰撞就可以删除，直到迭代到一轮下来没有点被剔除为止。