双轮差速模型的路径规划学习1-matlab模拟指定路径运动

本文为路径规划和matlab的学习随笔，主要参考matlab帮助文档里的“Path Following for a Differential Drive Robot”
操作环境：matlab2019b
使用模型：双轮差速小车模型

一、双轮差速运动学模型简介

本文中的双轮差速模型是一种简化的车辆运动模型，主要模拟位置矢量q=[x y θ]T、运动速度矢量u=[v w]T（w为瞬时角速度、v为瞬时线速度），两个矢量的关系如下式。

该模型的输入为运动速度矢量（线速度与角速度），输出为位置矢量。从而可以根据速度的变化得到小车模型下一时刻的位置。

matlab中的四个参数

• 车轮半径 wheelradius ：车轮的半径将影响运动时的速度与位置关系，默认为0.05
• 车轮速度范围wheelspeedrange：线速度和角速度的范围，默认为负无穷到正无穷
• 轮距trackwidth：两轮之间的距离，默认为0.2
• 运动输入VehicleInputs：matlab中有两种
  （1）“WheelSpeeds” 左右轮的速度
  （2） "VehicleSpeedHeadingRate"线速度和角速度（本文使用该输入方式）

调用该模型代码为：differentialDriveKinematics
例如本文使用的：

robot = differentialDriveKinematics("TrackWidth", 1, "VehicleInputs", "VehicleSpeedHeadingRate");

这里只设置了轮距和运动输入方式，其余参数为默认值。

二、Matlab模拟指定路径运动

1. 设置地图与路径

首先设置地图，这里使用的是matlab自带的地图例子exampleMaps，选择其中的simplemap，并命名为SimpleMap，可通过show函数查看。

load exampleMaps
map = binaryOccupancyMap(simpleMap);
refFigure = figure('Name','SimpleMap');
show(map);

然后，设置指定的路径，该地图的长宽为27*26，可以将路径设置为一个个的坐标点，然后用直线相连，这里使用小编自己设置的path，大家可以根据情况自行设置路径，只要修改以下代码path中的坐标即可

path = [ 4.0000    2.0000
    5.0119    1.3390
    6.2018    7.6010
    6.7355   11.9239
   10.3720   15.4578
   13.6292   14.6277
   17.3369   13.0502
   16.6829    5.2424
   19.5966    4.7733
   24.0000    3.0000];
figure(refFigure)
hold on
plot(path(:,1), path(:,2),'k--d') %作图
hold off

作图之后可以看见路径已经在图上标记出来了

2.配置双轮差速模型和控制器

将path中的第一个坐标点设置为小车模型的初始位置，最后一个坐标点设置为最终位置，初始的目标点设置为0；设置当前位置为初始位置与目标点之间。
然后用differentialDriveKinematics调用双轮差速小车模型

robotInitialLocation = path(1,:);
robotGoal = path(end,:);
initialOrientation = 0;
robotCurrentPose = [robotInitialLocation initialOrientation]';
robot = differentialDriveKinematics("TrackWidth", 1, "VehicleInputs", "VehicleSpeedHeadingRate");

接下来配置控制器和控制标准
这里使用controllerPurePursuit控制器，期望的线速度DesiredLinearVelocity=0.6,最大角速度为MaxAngularVelocity = 2;LookaheadDistance = 0.3（这个参数的意义小编一直没有弄懂…之后在更新吧~）;

controller = controllerPurePursuit;
controller.Waypoints = path;
controller.DesiredLinearVelocity = 0.6;
controller.MaxAngularVelocity = 2;
controller.LookaheadDistance = 0.3;

设置控制标准，从起点到终点，到达终点的位置误差半径在0.5以内

goalRadius = 0.1;
distanceToGoal = norm(robotInitialLocation - robotGoal);

3. 编写运动方程-沿已生成路径运动

这里直接使用参考的帮助文档中的程序了，不过因为参考文档里的第一个例子没有结合地图，小编在图像显示部分做了一点点修改

% Initialize the simulation loop
sampleTime = 0.1;
vizRate = rateControl(1/sampleTime);

% Initialize the figure
figure

% Determine vehicle frame size to most closely represent vehicle with plotTransforms
frameSize = robot.TrackWidth/0.8;

while( distanceToGoal > goalRadius )
    
    % Compute the controller outputs, i.e., the inputs to the robot
    [v, omega] = controller(robotCurrentPose);
    
    % Get the robot's velocity using controller inputs
    vel = derivative(robot, robotCurrentPose, [v omega]);
    
    % Update the current pose
    robotCurrentPose = robotCurrentPose + vel*sampleTime; 
    
    % Re-compute the distance to the goal
    distanceToGoal = norm(robotCurrentPose(1:2) - robotGoal(:));
    
    % Update the plot  %修改的地图显示部分
    hold off
    show(map);
    hold all     
    % Plot path each instance so that it stays persistent while robot mesh
    % moves
    plot(path(:,1), path(:,2),"k--d")    %修改的地图显示部分
    
    
    % Plot the path of the robot as a set of transforms
    plotTrVec = [robotCurrentPose(1:2); 0];
    plotRot = axang2quat([0 0 1 robotCurrentPose(3)]);
    plotTransforms(plotTrVec', plotRot, "MeshFilePath", "groundvehicle.stl", "Parent", gca, "View","2D", "FrameSize", frameSize);
    light;
    xlim([0 27])
    ylim([0 26])
%     xlim([0 13])
%     ylim([0 13])
    
    waitfor(vizRate);
end

4. 运行测试

运行之后可以看到小车模型从起点沿着路径运行到了终点

本文代码可以在以下链接中下载https://download.csdn.net/download/hongliyu_lvliyu/12259424

代码说明：两个都是m脚本文件，先运行pathflowing生成路径，然后再运行Untitle控制小车运动

*本文主要参考matlab帮助文档里的“**Path Following for a Differential Drive Robot”*如有错误，还请大家指出~ ！谢谢