该实施例表明如何控制一个机器人跟随使用机器人仿真的期望路径。该示例使用下列控制器来沿预定路径驱动模拟机器人纯追踪路径。期望的路径是明确定义或使用路径规划器计算一组路点(参照在不同复杂的环境路径规划)。为一个模拟差动驱动机器人纯追踪路径跟随控制器创建并计算控制命令遵循给定的路径。所计算的控制命令被用于驱动模拟机器人沿期望轨迹遵循基于纯追踪控制器上的期望的路径。
注:在开始R2016b,而不是使用步骤方法来执行由系统对象定义的操作™,你可以调用该对象与参数,就好像它是一个功能。例如,Y =步骤(OBJ,x)的
和Y = OBJ(x)的
执行类似的操作。
定义一组路点的用于机器人所需的路径
路径= [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 =路径(1,:);robotGoal =路径(端,:);
假定初始机器人取向(机器人取向是机器人标题和正X轴,测量反时针之间的角度)。
initialOrientation = 0;
定义当前姿势为机器人[X Y的2θ
robotCurrentPose = [robotInitialLocation initialOrientation]';
初始化机器人模型,并为其分配初始姿态。模拟的机器人具有用于两轮差动驱动机器人的运动运动学方程。此模拟机器人的输入是线速度和角速度。
机器人= differentialDriveKinematics(“道宽”1,“VehicleInputs”,“VehicleSpeedHeadingRate”);
可视化所需的路径
图图(路径(:,1),路径(:,2),数k - d')XLIM([0 13])ylim([0 13])
基于上述所定义的路径和一个机器人运动模型上,则需要以下控制器沿所述路径驱动所述机器人的路径。创建使用路径以下控制器controllerPurePursuit
宾语。
控制器= controllerPurePursuit;
使用上面所定义的路径来设定所希望的航点用于控制器
controller.Waypoints =路径;
以下控制器参数设置的路径。所期望的线速度被设定为0.3米/秒的这个例子。
controller.DesiredLinearVelocity = 0.6;
最大角速度用作饱和极限为旋转速度,它被设置为2弧度/秒的这个例子。
controller.MaxAngularVelocity = 2;
作为一般规则,先行距离应该比的平滑路径所需的线速度较大。当先行距离较大的机器人可能会偷工减料。相反,可能会导致以下行为不稳定路径的小超前距离。被选为0.5μm的值在这个例子中。
controller.LookaheadDistance = 0.3;
路径下列控制器提供用于机器人的输入控制信号,机器人的用途,以驱动本身沿期望路径哪个。
定义一个目标半径,这是机器人的最终位置和所述目标位置之间的期望距离阈值。一旦机器人是从目标这个距离内,它就会停止。此外,您计算机器人的位置和目标位置之间的当前距离。这个距离在球门半径不断检查,当该距离小于目标半径机器人停止。
注意目标半径是太小的值可能导致机器人错过目标,这可能导致在靠近目标的一个意外的行为。
goalRadius = 0.1;distanceToGoal =规范(robotInitialLocation - robotGoal);
该controllerPurePursuit
对象计算用于机器人控制命令。直到它到达目标半径范围内使用这些控制命令驱动机器人。如果使用的是一个外部模拟器或物理机器人,则控制器输出应该施加给机器人和定位系统可能需要更新所述机器人的姿态。控制器在10Hz运行。
%初始化仿真循环sampleTime = 0.1;vizRate =码率控制(1 / sampleTime);%初始化图数字%确定车辆帧大小来最密切plotTransforms表示车辆框架尺寸= robot.TrackWidth / 0.8;而(distanceToGoal> goalRadius)%计算控制器输出的,即,输入到机器人[V,ω=控制器(robotCurrentPose);%获取机器人的使用速度控制器输入VEL =衍生物(机器人,robotCurrentPose,[Vω);%更新当前位姿robotCurrentPose = robotCurrentPose + VEL * sampleTime;%重新计算到目标的距离distanceToGoal =范数(robotCurrentPose(1:2) - robotGoal(:));%更新剧情保持离%绘路径每个实例,使其保持持久的,而机器人网移动%图(路径(:,1),路径(:,2),“K - d”)保持所有%绘制机器人的路径为一组变换plotTrVec = [robotCurrentPose(1:2);0];plotRot = axang2quat([0 0 1 robotCurrentPose(3)]);plotTransforms(plotTrVec”,plotRot,“MeshFilePath”,“groundvehicle.stl”,“家长”,GCA,“视图”,“2D”,“框架尺寸”,框架尺寸);光;XLIM([0 13])ylim([0 13])WAITFOR(vizRate);结束
如果路标的期望的一组是由路径规划计算时,路径跟踪控制器可以以相同的方式使用。首先,可视化地图
加载exampleMapsMAP = binaryOccupancyMap(simpleMap);图显示(地图)
你可以计算出路径
使用PRM路径规划算法。看到在不同复杂的环境路径规划了解详情。
mapInflated =拷贝(地图);膨胀(mapInflated,robot.TrackWidth / 2);PRM = robotics.PRM(mapInflated);prm.NumNodes = 100;prm.ConnectionDistance = 10;
查找开始和结束位置之间的路径。注意,路径
会有所不同,由于PRM算法的概率性质。
startLocation = [4.0 2.0]。EndLocation所= [24.0 20.0];路径= 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
显示充气地图,道路地图,并最终路径。
显示(PRM);
你以下控制器之上,你可以重新使用用于计算这个地图上的机器人的控制命令限定的路径。为了重新使用控制器并重新定义航点同时保持其它信息相同,则使用释放
功能。
释放(控制器);controller.Waypoints =路径;
设置由路径所限定的初始位置和机器人的目标
robotInitialLocation =路径(1,:);robotGoal =路径(端,:);
假定初始机器人取向
initialOrientation = 0;
限定用于机器人的运动[x和y的2θ当前姿态
robotCurrentPose = [robotInitialLocation initialOrientation]';
到目标位置计算距离
distanceToGoal =规范(robotInitialLocation - robotGoal);
定义一个目标半径
goalRadius = 0.1;
使用给定的地图上的控制器输出,直到它到达目标驱动机器人。控制器在10Hz运行。
复位(vizRate);%初始化图数字而(distanceToGoal> goalRadius)%计算控制器输出的,即,输入到机器人[V,ω=控制器(robotCurrentPose);%获取机器人的使用速度控制器输入VEL =衍生物(机器人,robotCurrentPose,[Vω);%更新当前位姿robotCurrentPose = robotCurrentPose + VEL * sampleTime;%重新计算到目标的距离distanceToGoal =范数(robotCurrentPose(1:2) - robotGoal(:));%更新剧情保持离显示地图);保持所有%绘路径每个实例,使其保持持久的,而机器人网移动%图(路径(:,1),路径(:,2),“K - d”)%绘制机器人的路径为一组变换plotTrVec = [robotCurrentPose(1:2);0];plotRot = axang2quat([0 0 1 robotCurrentPose(3)]);plotTransforms(plotTrVec”,plotRot,'MeshFilePath','groundvehicle.stl',“父”,GCA,“视图”,“2D”,“框架尺寸”,框架尺寸);光;XLIM([0 27])ylim([0 26])WAITFOR(vizRate);结束