路径优化

最近学习凸优化和其他优化算法，做个例子

优化目标：路程

基于不等式约束最优路径

clc
clear all
close all

A = [1, 9];%起点

P = [1, 7
     3, 6
     5, 8
     4, 5
     5, 3
     8, 5
     8, 2];%初始化路径，可以随便定义
B = [9, 1];%终点
C = [2, 8;
     3, 6
     8, 6
     8, 2];%障碍物坐标
r = [ 0.8;
      0.7
      1
      0.5];%障碍物半径
lb = [0 0; 0 0; 0 0; 0 0; 0 0;0 0;0 0; ];%对应初始路径想，x，y最小范围
ub = [10 10; 10 10;10 10;10 10; 10 10;10 10; 10 10];%对应初始路径想，x，y最大范围

X = fmincon(@(P)objective(A,P,B), P, [],[],[],[], lb, ub,@(P)constarint(A, P, B, C, r))
%目标函数模型、初始可行解、线性不等式A、线性不等式b、线性等式A、线性等式b、xy下界、xy上界、
%非线性等式与不等式
draw(A, P, B ,X, C, r) 


function f = objective(A,P,B)
    f3 = zeros(1, size(P,1));
    f1 = norm( P(1,:) - A  );
    f2 = norm( B - P(end,:) );
     for i = 1 : size(P,1) - 1
        f3(i) = norm( P(i,:)-P(i+1,:) );
     end
     f4 = sum(f3);
     f = f1 + f2 + f4;%目标函数为路程，也可加入转角等。
end 


function draw(A, P, B ,X, C, r)
xlim([0,10])
ylim([0,10])

for i = 1 : size(C,1)
    viscircles(C(i, :), r(i)) ;
end
hold on       
grid on
M = [A; P; B];
M1= [A; X; B];
labels = {'A', '1', '2' '3', '4', '5', '6', '7', 'B'};
plot (M(:, 1),M(:,2), '--or')
plot (M1(:, 1),M1(:,2),'-ob')
hold off
legend('Non optimal', 'Optimal')
end

function [g,h] = constarint(A, P, B, C, r)
    X = [A ; P ; B ];
    n = 0: 0.1 :1;
    y = zeros();
    g = zeros();
    k = 1;
    for i = 1:size(X,1)-1
        for k = 1 : size(C,1)
             for j = 1 : length(n)
                finite = X(i,:) + n(j) * (X(i+1,:)-X(i,:));  
                y(j,i) = norm(C(k,:) - finite);
             end
                g(i,k) = r(k) -  min( y(:, i));      
        end
    end%避免与圆形障碍物冲突
    h= [];
end