2021-06-16 基于差速机器人的路径跟踪（一）

最近在研究基于差速机器人的路径跟踪，先实现简单的路径跟踪功能，后期会在其基础上实现SLAM功能，在这里记录一下。

差速机器人运动模型

如图所示为差速机器人模型，描述小车的速度一般为小车质心的线速度 $v$和角速度$\omega$。小车在笛卡尔坐标系下的坐标为$(x,y)$，航向角为$\theta$。差速车的运动模型为：
$$\{\begin{array}{rcl}\dot{x}& =& v\cos (\theta )\\ \dot{y}& =& v\sin (\theta )\\ \dot{\theta }& =& \omega \end{array}$$
在ROS框架下，小车的控制指令一般为质心线速度 $v$和角速度$\omega$，但是小车的底层控制以控制两轮速度为主，所以需要对速度进行反解算。假设小车左右轮线速度分别为$v_l$和$v_r$，轮间距为$L$，因为小车转动时两轮是同轴转动（角速度相同：${\omega }_l$=${\omega }_r$=$\omega$），所以可以得出：
$$\begin{gathered} L=\frac{v_l}{{\omega }_l}-\frac{v_r}{{\omega }_r} \\ \omega =\frac{v_l-v_r}{L} \end{gathered}$$
质心线速度和两轮线速度的关系为：
$$v=\frac{v_l+v_r}{2}$$
由此，可以得出两轮线速度为：
$$\begin{gathered} v_l=v+\frac{\omega l}{2} \\ {v}_r=v-\frac{\omega l}{2} \end{gathered}$$

Nonlinear Guidance Law（非线性导引控制率）

NLGL算法是在纯跟踪算法的基础上推导得出的，纯跟踪算法的基本原理可以参照：https://zhuanlan.zhihu.com/p/112065159
https://zhuanlan.zhihu.com/p/377036466
NLGL算法的控制律为：
$$a_{Scmd}=2\frac{v^2}{L_1}sin(\eta )$$

其中，$a_{Scmd}$为运动体的横向加速度，也可以视为向心加速度，$L_1$为运动体的前视距离（需要自己设定），$v$为运动体的质心线速度，$\eta$为前视距离$L_1$与质心线速度$v$的夹角。
由纯跟踪算法可知，运动体的实时旋转半径$R=\frac{L_1^2}{2x}$，$x$为在车体坐标系下目标点到运动体质心的横向偏差，$x=L_{1}sin(\eta )$，由此可得$R=\frac{L_1}{2sin(\eta )}$。已知运动体的质心线速度$v$，则向心加速度$a_{Scmd}=\frac{v^2}{R}$，由此得到NLGL算法的控制律。

MATLAB仿真

不管是在纯跟踪算法还是NLGL算法中，前视距离的选择关系到小车的路径跟踪效果，选择的前视距离太短导致无法与路径相交，就无法获取到目标点，导致小车运动在短时间内振荡；选择的前视距离太长，虽然跟踪路径会比较平滑，但是会产生较大的跟踪误差。因此，前视距离需要根据小车的运动情况和路径的变化不断做出调整。具体的前视距离的选择方法主要参考：pure pursuit预瞄距离的改进思路方法介绍 1。
具体的代码如下所示：

%% 参数设定:确定前视距离的方法 最简单的比例关系
clear; clc ;
global Lf; %前视距离
Lf = 0.04;
eta = 0;
kp = 0.8; %速度增益
dt = 0.01; %[s] 时间间隔
T = 100; %最大仿真时间

%% 参考路径 圆 闭合曲线
global r;
aplha=0:pi/100:2*pi;
r=0.4;
cx=r*cos(aplha);
cy=r*sin(aplha);
global max_Index;
max_Index = length(cx);
%% 初始状态设定 
i = 1;
j = 1;
x = zeros(1,10); 
y = zeros(1,10);
yaw = zeros(1,10); %航向角
v = zeros(1,10); %速度
%初始位置设定
x(1) = 0.4;
y(1) = 0; 
yaw(1) = 1.5;
global speed;
speed = 0.1; %速度设定

t = zeros(size(cx)); %仿真时间
index = zeros(size(cx)); %索引值

global oldnearestp_ind; % 存储上一时刻临近点的索引
oldnearestp_ind = -1; 
global target_ind; % 目标点索引
target_ind = 0;

%% 运行主体
[target_ind]= calc_target_index(x(1),y(1),cx,cy);
index(1) = target_ind;
while ((T >= t(i)) && (max_Index >= target_ind)) 
     if(target_ind==max_Index)  % 判断是否到达路径终点
        dis = calc_distance(x(i),y(i),cx(target_ind),cy(target_ind));
        if(dis<=0.02)
            v(i+1) = 0; w = 0;
            break;
        end
    end 
    [w] = pure_pursuit_control(x(i),y(i),yaw(i),v(i),cx,cy,target_ind);
    index(j+1) = target_ind;

    [a] = speedControl(kp,v(i));
    [x(i+1),y(i+1),yaw(i+1),v(i+1)] = updatestate(x(i),y(i),yaw(i),v(i),a,w,dt);
    t(j+1) = t(j)+dt;
    i = i+1;
    j = j+1;
end
%% 绘图
figure;
plot(cx,cy,'b'); hold on;
plot(x,y,'r','Linewidth',1);
legend('ref','fact');

function [distance] = calc_distance(x,y,cx,cy) 
    distance = sqrt((cx-x)^2+(cy-y)^2);
end

function [ind] = calc_nearest_index(x,y,cx,cy)
    global oldnearestp_ind;
    global max_Index;
    N =  max_Index; 
    if oldnearestp_ind == -1 % 初始时刻搜寻最近路径点
         Distance = zeros(1,N);  
         for i = 1:N
            Distance(i) =  sqrt((cx(i)-x)^2 + (cy(i)-y)^2); 
         end
         [~,ind] = min(Distance); 
          oldnearestp_ind = ind;
    else
        ind =  oldnearestp_ind;
        distance_this_index = calc_distance(x,y,cx(ind),cy(ind));
        while (ind+1) <= N %判断索引值是否超过路径点总体长度
            distance_next_index = calc_distance(x,y,cx(ind+1),cy(ind+1));
            if distance_this_index < distance_next_index
                break;
            end
            ind = ind + 1;
            distance_this_index = distance_next_index;
        end
        oldnearestp_ind = ind;
    end  
end

function [ind] = calc_target_index(x,y,cx,cy)
    global Lf;
    [ind] = calc_nearest_index(x,y, cx,cy);
    L = calc_distance(x,y,cx(ind),cy(ind));
    while Lf>=L && (ind+1)<=length(cx)
        ind = ind+1;
        L = calc_distance(x,y,cx(ind),cy(ind));
    end   
end

function [w, ind] = pure_pursuit_control(x,y,yaw,v,cx,cy,pind)
    global Lf;
    global target_ind;
    global max_index;
    global eta;
    [ind] = calc_target_index(x,y,cx,cy);
    if pind >= ind
        target_ind = pind;
    else
        target_ind = ind;
    end
    if target_ind <= length(cx)
        tx = cx(target_ind);
        ty = cy(target_ind);
    else
        tx = cx(length(max_index));
        ty = cy(length(max_index)); 
    end 
        theta = atan2((ty-y), (tx-x)); %目标点与质心坐标的向量与x轴的夹角
        eta = theta-yaw;
        w = (2*v*sin(eta))/Lf;
end

仿真效果