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

最新推荐文章于 2024-09-28 12:59:32 发布

豆豆Qiang

最新推荐文章于 2024-09-28 12:59:32 发布

阅读量5.4k

点赞数 9

文章标签：算法 matlab

本文链接：https://blog.csdn.net/ww12wy/article/details/117966646

版权

本文介绍了基于差速机器人的路径跟踪技术，重点探讨了非线性导引控制率（NLGL）算法，并提供了MATLAB仿真的详细过程。通过设置合适的前视距离，实现了小车对圆闭合曲线路径的有效跟踪。仿真结果显示，NLGL算法能够使小车平稳地跟随路径，并讨论了前视距离对跟踪效果的影响。

摘要由CSDN通过智能技术生成

基于差速机器人的路径跟踪（一）

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

差速机器人运动模型

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

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$ 的夹角。
由纯跟踪算法可知，运动体的实时旋转半径 $\frac{L_1^2}{2x}$ ， $x$ 为在车体坐标系下目标点到运动体质心的横向偏差， $x=L_1sin(\eta)$ ，由此可得 $\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