轮式移动机器人位姿闭环控制MATLAB仿真

clc; clear; close all;
n = 16; % trial num
d = 10; % initial distance

xi_target = [0;0;0];

angle_init = linspace(0, 2*pi*(1-1/n), n);

x_init = d * cos(angle_init);
y_init = d * sin(angle_init);
theta0 = 0;
theta_init = theta0 * ones(n,1);

xi_init = [x_init',y_init',theta_init];

%% Run n trials
xi_record = cell(n,1);
for i = 1:n
    xi_record{i} = one_trial(xi_target, xi_init(i,:)');
end

%% Plot results
% Plot path
figure;
for i = 1:n
    color = rand(1,3);
    plot(xi_record{i}(:,1), xi_record{i}(:,2), 'linewidth', 1, 'color', color); hold on;
    quiver(xi_record{i}(1,1),xi_record{i}(1,2),cos(theta0),sin(theta0),'LineWidth',1,'MaxHeadSize',2, 'color', color); hold on;
end
quiver(xi_target(1),xi_target(2),cos(xi_target(3)),sin(xi_target(3)),'LineWidth',2,'MaxHeadSize',2,'color','k');
axis equal;
set(gca,'LooseInset',get(gca,'TightInset'));

% Plot xi-t
figure;
demo_index = 4;
time_series = 0.001*(1:size(xi_record{demo_index},1));
yyaxis left; % 激活左边的轴
plot(time_series, xi_record{demo_index}(:,1),'linewidth',1); hold on;
plot(time_series, xi_record{demo_index}(:,2),'linewidth',1);
xlabel('t / s');
ylabel('position / m');

yyaxis right; % 激活右边的轴
plot(time_series, xi_record{demo_index}(:,3),'linewidth',1); hold on;
ylabel('\theta / rad');
legend('x(t)','y(t)','\theta(t)');
set(gca,'LooseInset',get(gca,'TightInset'));


%% function: run single trial
function xi_record = one_trial(xi_target, xi_init)

%% Precondition: check if moving backforward is easier, if so, swap xi_target and xi_init (equals to v < 0)
x = xi_target(1) - xi_init(1);
y = xi_target(2) - xi_init(2);
theta = xi_init(3);

swapped = 0;
if abs(atan2(y,x) - theta) > pi/2
    swapped = 1;
    % Swap xi_target and xi_init
    tmp = xi_target;
    xi_target = xi_init;
    xi_init = tmp;
end

%% Start iteration
xi_record = zeros(10000,3);
k = 0;
xi = xi_init;
xi_record(k+1,:) = xi';

error = xi_target - xi;

while norm(error) > 0.1
    k = k + 1;
    
    %% 差动机器人
    d_phi = diff_controller(xi, xi_target);
    xi = diff_model(xi, d_phi);
    
    %% 麦轮机器人
%     d_phi = mec_controller(xi, xi_target);
%     xi = mec_model(xi, d_phi);
    
    xi_record(k+1,:) = xi';
    error = xi_target - xi;
end

xi_record = xi_record(1:k+1,:);

if swapped
    xi_record = flipud(xi_record);
end

end

%% 差动机器人物理模型
function xi_next = diff_model(xi, d_phi)
% input: xi = [x;y;theta] - current state in world coordinate frame, 3*1 vector;
% input: d_phi - 2*1 vector
% output: xi_next - next state in world coordinate frame, 3*1 vector;
r = 0.2;
l = 0.5;
delta_t = 0.001;

theta = xi(3);
R = [cos(theta), -sin(theta), 0;
    sin(theta),  cos(theta), 0;
    0,           0, 1];
d_xi_R = [r/2, r/2; 0, 0; r/(2*l), -r/(2*l)] * d_phi;
d_xi = R * d_xi_R;
xi_next = xi + d_xi * delta_t;
end

%% 麦轮机器人物理模型
function xi_next = mec_model(xi, d_phi)
% input: xi = [x;y;theta] - current state in world coordinate frame, 3*1 vector;
% input: d_phi - 2*1 vector
% output: xi_next - next state in world coordinate frame, 3*1 vector;
r = 0.2;
l = 0.5;
delta_t = 0.001;

theta = xi(3);
R = [cos(theta), -sin(theta), 0;
    sin(theta),  cos(theta), 0;
    0,           0, 1];
d_xi_R = [r/sqrt(3),        0, -r/sqrt(3);
    -r/3,    2*r/3,       -r/3;
    -r/(3*l), -r/(3*l), -r/(3*l)] * d_phi;
d_xi = R * d_xi_R;
xi_next = xi + d_xi * delta_t;
end

%% 差动机器人Controller
function d_phi = diff_controller(xi, xi_target)
% input: error = [x;y;theta] - current state error
% output: d_phi - 2*1 vector
%% Parameters
r = 0.2;
l = 0.5;
k_rho = 3;
k_alpha = 8;
k_beta = -1.5;

%% Convert error [x; y; theta] to [rho; alpha; beta]
x = xi_target(1) - xi(1);
y = xi_target(2) - xi(2);
theta = xi(3);
theta_target = xi_target(3);

rho = sqrt(x^2 + y^2);
alpha = -theta + atan2(y,x);
beta = -theta - alpha + theta_target;

%% P-control
v = k_rho * rho;
omega = k_alpha * alpha + k_beta * beta;
d_xi_R = [v; 0; omega];
d_phi = 1/r * [1, 0, l; 1, 0, -l] * d_xi_R;
end

%% 麦轮Controller
function d_phi = mec_controller(xi, xi_target)
% input: error = [x;y;theta] - current state error
% output: d_phi - 3*1 vector
%% Parameters
r = 0.2;
l = 0.5;
K = diag([5,5,1]);

%% Convert error [x; y; theta] to [rho; alpha; beta]
theta = xi(3);
error = xi_target - xi;
u = K * error; % P-control
R = [cos(theta), -sin(theta), 0;
    sin(theta),  cos(theta), 0;
    0,           0, 1];
d_xi_R = R^(-1) * u;
d_phi = [sqrt(3)/(2*r), -1/(2*r), -l/r;
    0,      1/r, -l/r;
    -sqrt(3)/(2*r), -1/(2*r), -l/r] * d_xi_R;

end

demo

二轮差动机器人，从距离原点等距离的1024个初始位置（初始角度均为45度），收敛到目标位姿(0,0,0)。