【移动机器人技术】轨迹跟踪器之——比例调节率器设计

0 概述

本文介绍一种轨迹跟踪调节算法，属于简洁的比例调节器。
本算法来自timed_path_follower模块；
https://github.com/marinaKollmitz/human_aware_navigation/wiki；
下文中需要用到的几何分析图如下：

算法流程

轨迹点匹配

基于时间戳匹配轨迹上的参考点及速度
findWaypointAtTime(now, waypoint, waypoint_vel)；
tf::Pose waypnt;
tf::poseMsgToTF(waypoint.pose, waypnt);

坐标变换：将机器人变换至目标点坐标系下

tf::Pose robot_in_wpnt = waypnt.inverse() * robot_pose;
机器人在目标点坐标系下的坐标及方位角度：
double delta_x = robot_in_wpnt.getOrigin().getX();
double delta_y = robot_in_wpnt.getOrigin().getY();
double phi = tf::getYaw(robot_in_wpnt.getRotation());

计算偏差量

double rho = hypot(delta_x, delta_y);
double alpha = atan2(-1delta_y, -1delta_x) - phi;
double beta = -1* alpha - phi;

速度调节率：

cmd_vel.linear.x = k_rho_ * rho;
cmd_vel.angular.z = k_alpha_ * alpha + k_beta_ * beta;

仿真

比例调节的基本特点

速度与偏差呈现比例关系，需要有偏差才能有速度，所以动态偏差永远存在；

m文件

调度文件

clc;
close all;
clear all;
%% 参考轨迹生成
N=200;%参考轨迹点数量
T=0.05;%采样时间，控制周期

Xref=zeros(N,3);
T_axis=zeros(N,1);
v_ref=zeros(N,1);
w_ref=zeros(N,1);

Xref(1,1) = 0.0;
Xref(1,2) = 0.2;
Xref(1,3) = 0.1;

x_real=zeros(N,3);
x_real(1,1) = 0.0;
x_real(1,2) = 0.0;
x_real(1,3) = Xref(1,3) - 0.2;

v_real=zeros(N,1);
w_real=zeros(N,1);
for k=2:1:N
    t = (k-1)*T;
    
    v_ref(k) = 0.3;
    w_ref(k) = 0.6*sin(3*t);

    phi = Xref(k-1,3);
    Xref(k,1) = Xref(k-1,1) + v_ref(k)*cos(phi)*T;
    Xref(k,2) = Xref(k-1,2)+ v_ref(k)*sin(phi)*T;
    Xref(k,3) = phi + w_ref(k)*T;
    if Xref(k,3) > pi
        Xref(k,3) = Xref(k,3) - 2*pi;
    end
    if Xref(k,3) < -pi
        Xref(k,3) = Xref(k,3) + 2*pi;
    end
    T_axis(k,1)=t;
    
    traj_point = [Xref(k,1), Xref(k,2), Xref(k,3), v_ref(k), w_ref(k)];
    real_point = [x_real(k-1,1), x_real(k-1,2), x_real(k-1,3), v_real(k-1), w_real(k-1)];
    
    [vd11, vd22] = Regulator_Gain(traj_point, real_point);
%     vd11,vd22
    v_real(k) = vd11;
    w_real(k) = vd22;
    % -----------------------------------------------------
    % update the simuliation(real) model vars:
    phi_real = x_real(k-1,3);
    x_real(k,1) = x_real(k-1,1) + v_real(k)*cos(phi_real)*T;
    x_real(k,2) = x_real(k-1,2) + v_real(k)*sin(phi_real)*T;
    x_real(k,3) = phi_real + w_real(k)*T; 
    if x_real(k,3) > pi
        x_real(k,3) = x_real(k,3) - 2*pi;
    end
    if x_real(k,3) < -pi
        x_real(k,3) = x_real(k,3) + 2*pi;
    end
%     xc = x_real(k+1,1)
%     yc = x_real(k+1,2)
%     thc = x_real(k+1,3)
     
    figure(1);
    plot(Xref(k,1),Xref(k,2), 'r*'); hold on;
    plot(x_real(k,1),x_real(k,2), 'g*'); hold on;
end
%% 以下为绘图部分
%-----------------------------------------------------
figure(2)
subplot(3,1,1);
plot(T_axis(1:N),Xref(1:N,1),'k--');hold on;
plot(T_axis(1:N),x_real(1:N,1),'k');hold on;
%grid on;
xlabel('采样时间T');
ylabel('横向位置X')
subplot(3,1,2);
plot(T_axis(1:N),Xref(1:N,2),'k--');hold on;
plot(T_axis(1:N),x_real(1:N,2),'k');hold on;
%grid on;
xlabel('采样时间T');
ylabel('纵向位置Y')
subplot(3,1,3);
plot(T_axis(1:N),Xref(1:N,3),'k--');hold on;
plot(T_axis(1:N),x_real(1:N,3),'k');hold on;
%grid on;
xlabel('采样时间T');
ylabel('\theta')
%----------------------------------------------------- 
figure(3)
subplot(2,1,1);
plot(T_axis(1:N),v_real(1:N),'k');
%grid on;
xlabel('采样时间T');
ylabel('纵向速度')
subplot(2,1,2)
plot(T_axis(1:N),w_real(1:N),'k');
%grid on;
xlabel('采样时间T');
ylabel('角速度')

调节器函数

function [v_ref, w_ref] = Regulator_Gain(traj_point, real_point)
%% constant
k_tho = 2.5;
k_alpha = 4.2;
k_beta = 1.8*1.5;

k_ff = 0.0;
%% input
traj_x = traj_point(1);
traj_y = traj_point(2);
traj_th = traj_point(3);
traj_v = traj_point(4);
traj_w = traj_point(5);

real_x = real_point(1);
real_y = real_point(2);
real_th = real_point(3);
real_v = real_point(4);
real_w = real_point(5);

%% regulator
% translation

T_traj = [cos(traj_th), -sin(traj_th), 0, traj_x;
              sin(traj_th),  cos(traj_th), 0, traj_y;
                           0,                0,    1,     0;
                           0,                0,    0,    1;];
                       
T_robot = [cos(real_th), -sin(real_th), 0, real_x;
              sin(real_th),  cos(real_th), 0, real_y;
                           0,                0,    1,     0;
                           0,                0,    0,    1;];
% Replace inv(A)*b with A\b
% Replace b*inv(A) with b/A
%  T_robot_in_traj  = inv(T_traj)*T_robot;
T_robot_in_traj  = T_traj\T_robot;
 
 new_x = T_robot_in_traj(1,4);
 new_y = T_robot_in_traj(2,4);
 new_th = atan2(T_robot_in_traj(2,1), T_robot_in_traj(1,1));
 
% error
dx = new_x;
dy = new_y;
ds = hypot(dx, dy);
beta = - atan2(-dy, -dx);
alpha = -new_th - beta;

if alpha > pi
    alpha = alpha - 2*pi;
end

if  alpha < -pi
    alpha = alpha +2*pi;
end

% regulator
if (alpha < 0.5*pi) && (alpha > -0.5*pi)
    v_reg = k_tho*ds;
else
    v_reg = 0;
end
w_reg = k_alpha * alpha + k_beta * beta;

v_ff = k_ff*traj_v;
w_ff = k_ff*traj_w;

v = v_reg + v_ff;
w = w_reg + w_ff;

%% output
if v > 1.0
    v = 1.0;
end
if v < -1.0
    v = -1.0;
end
if w > 1.0
    w = 1.0;
end
if w < -1.0
    w = -1.0;
end
v_ref = v;
w_ref = w;


end