差动驱动机器人的路径跟踪
本示例演示如何使用机器人模拟器控制机器人沿着所需的路径运行。本例使用Pure Pursuit路径跟踪控制器沿预定路径驱动模拟机器人。理想路径是一组路径点,通过路径规划器显式定义或计算(请参阅不同复杂环境下的路径规划).为仿真差动驱动机器人创建了Pure Pursuit路径跟踪控制器,并计算控制命令以跟踪给定的路径。计算出的控制命令用于驱动模拟机器人沿着期望的轨迹,并遵循基于Pure Pursuit控制器的期望路径。
注意:从R2016b开始,不再使用step方法执行System对象™定义的操作,而是使用参数调用对象,就好像它是一个函数一样。例如,Y = step(obj,x)
而且Y = obj(x)
请执行相同的操作。
定义锚点
为机器人的期望路径定义一组路径点
Path = [2.00 1.00;1.25 - 1.75;5.25 - 8.25;7.25 - 8.75;11.75 - 10.75;12.00 - 10.00);
设置路径定义的机器人当前位置和目标位置。
robotInitialLocation = path(1,:);robotGoal = path(end,:);
假设一个初始机器人方向(机器人方向是机器人方向与正x轴之间的角度,逆时针测量)。
initialOrientation = 0;
定义机器人的当前姿态[x y theta]
robotCurrentPose = [robotInitialLocation initialOrientation]';
创建一个运动学机器人模型
初始化机器人模型并分配初始姿态。仿真机器人具有两轮差动驱动机器人运动的运动学方程。这个模拟机器人的输入是线速度和角速度。
机器人=微分驱动器运动学(“TrackWidth”, 1“VehicleInputs”,“VehicleSpeedHeadingRate”);
想象理想的路径
图(path(:,1), path(:,2),“k - d ') xlim([0 13]) ylim([0 13])
定义路径跟随控制器
基于上面定义的路径和机器人运动模型,需要一个路径跟踪控制器来驱动机器人沿着路径。控件创建控制器下面的路径controllerPurePursuit
对象。
controller = controllerPurePursuit;
使用上面定义的路径为控制器设置所需的路径点
控制器。路点=路径;
根据控制器参数设置路径。在本例中,期望的线速度设置为0.6米/秒。
控制器。期望线性速度= 0.6;
最大角速度作为旋转速度的饱和限制,在本例中设置为2弧度/秒。
控制器。MaxAngularVelocity = 2;
作为一般规则,前视距离应该大于平滑路径所需的线速度。当前视距离较大时,机器人可能会抄近路。相反,小的前视距离会导致不稳定的路径跟随行为。本例中选择的值为0.3 m。
控制器。LookaheadDistance = 0.3;
使用路径跟踪控制器,驱动机器人经过所需的路径点
路径跟踪控制器为机器人提供输入控制信号,机器人利用这些信号沿期望的路径驱动自身。
定义目标半径,即机器人最终位置与目标位置之间的期望距离阈值。一旦机器人距离目标有了这个距离,它就会停下来。此外,您还计算机器人位置和目标位置之间的当前距离。这个距离会根据目标半径不断检查,当这个距离小于目标半径时机器人就会停止。
注意,目标半径值过小可能会导致机器人错过目标,从而可能导致在目标附近出现意外行为。
goalRadius = 0.1;distanceToGoal = norm(robotInitialLocation - robotGoal);
的controllerPurePursuit
对象为机器人计算控制命令。使用这些控制命令驱动机器人,直到它到达目标半径内。如果您正在使用外部模拟器或物理机器人,则应将控制器输出应用于机器人,并且可能需要定位系统来更新机器人的姿态。控制器运行频率为10hz。
初始化模拟循环sampleTime = 0.1;vizRate = rateControl(1/sampleTime);初始化图形数字确定车辆框架大小,以最接近地表示车辆与plotTransformsframeSize = robot.TrackWidth/0.8;而(distanceToGoal > goalRadius)计算控制器输出,即机器人的输入。[v, omega] = controller(robotCurrentPose);使用控制器输入获取机器人的速度vel =导数(机器人,robotCurrentPose, [v ω]);更新当前姿态robotCurrentPose = robotCurrentPose + vel*sampleTime;重新计算到目标的距离distanceToGoal = norm(robotCurrentPose(1:2) - robotGoal(:));%更新绘图持有从绘制每个实例的路径,以便在机器人网格时保持持久性%的举措情节(路径(:1),路径(:,2),“k - d”)举行所有将机器人的路径绘制为一组变换plotTrVec = [robotCurrentPose(1:2);0);plotRot = axang2quat([0 0 1 robotCurrentPose(3)]);plotRot plotTransforms (plotTrVec ',“MeshFilePath”,“groundvehicle.stl”,“父”甘氨胆酸,,“视图”,“二维”,“FrameSize”, frameSize);淡定;xlim([0 13]) ylim([0 13]) waitfor(vizRate);结束
使用路径跟踪控制器和PRM
如果所需的路径点集是由路径规划器计算的,路径跟踪控制器可以以同样的方式使用。首先,想象地图
负载exampleMapsmap = binaryoccuancymap (simpleMap);图显示(图)
你可以计算路径
采用PRM路径规划算法。看到不同复杂环境下的路径规划获取详细信息。
mapinflation = copy(map);充气(mapInflated, robot.TrackWidth / 2);prm = robotics.PRM(mapinflation);人口、难民和移民事务局。NumNodes = 100;人口、难民和移民事务局。ConnectionDistance = 10;
找到开始位置和结束位置之间的路径。注意路径
会因PRM算法的概率性质而有所不同。
startLocation = [4.0 2.0];endLocation = [24.0 20.0];path = findpath(prm, startLocation, endLocation)
路径=8×24.0000 2.0000 3.1703 2.7616 7.0797 11.2229 8.1337 13.4835 14.0707 17.3248 16.8068 18.7834 24.4564 20.6514 24.0000 20.0000
显示膨胀的地图、路线图和最终路径。
显示(人口、难民和移民事务局);
您定义了一个路径跟随控制器,您可以在此地图上重新使用该控制器来计算机器人的控制命令。若要重用控制器并重新定义路径点,同时保持其他信息相同,请使用释放
函数。
释放(控制器);控制器。路点=路径;
根据路径定义,设置机器人的初始位置和目标
robotInitialLocation = path(1,:);robotGoal = path(end,:);
假设机器人的初始方向
initialOrientation = 0;
定义机器人运动的当前姿态[x y theta]
robotCurrentPose = [robotInitialLocation initialOrientation]';
计算到目标位置的距离
distanceToGoal = norm(robotInitialLocation - robotGoal);
定义目标半径
goalRadius = 0.1;
使用给定地图上的控制器输出驱动机器人,直到它达到目标。控制器运行频率为10hz。
重置(vizRate);初始化图形数字而(distanceToGoal > goalRadius)计算控制器输出,即机器人的输入。[v, omega] = controller(robotCurrentPose);使用控制器输入获取机器人的速度vel =导数(机器人,robotCurrentPose, [v ω]);更新当前姿态robotCurrentPose = robotCurrentPose + vel*sampleTime;重新计算到目标的距离distanceToGoal = norm(robotCurrentPose(1:2) - robotGoal(:));%更新绘图持有从显示(地图);持有所有绘制每个实例的路径,以便在机器人网格时保持持久性%的举措情节(路径(:1),路径(:,2),“k - d”)将机器人的路径绘制为一组变换plotTrVec = [robotCurrentPose(1:2);0);plotRot = axang2quat([0 0 1 robotCurrentPose(3)]);plotRot plotTransforms (plotTrVec ',“MeshFilePath”,“groundvehicle.stl”,“父”甘氨胆酸,,“视图”,“二维”,“FrameSize”, frameSize);淡定;xlim([0 27]) ylim([0 26]) waitfor(vizRate);结束