这个例子演示了如何使用机器人模拟器来控制机器人沿着期望的路径前进。该示例使用Pure Pursuit路径跟踪控制器驱动模拟机器人沿预定路径前进。所需的路径是一组明确定义或使用路径规划器计算的路径点(参见不同复杂度环境下的路径规划).建立了仿真差动驱动机器人的Pure Pursuit路径跟踪控制器,并计算出了跟踪给定路径的控制指令。基于Pure Pursuit控制器,利用计算得到的控制命令驱动仿真机器人沿期望的轨迹跟随期望的路径。
注意:从R2016b开始,不再使用step方法执行System对象™定义的操作,而是可以使用参数调用对象,就像它是一个函数一样。例如,y =步骤(obj, x)
和y = obj (x)
执行相同操作。
为机器人的所需路径定义一组航路点
路径=[2.01.00;1.251.75;5.258.25;7.258.75;11.7510.75;12.0010.00];
根据路径定义,设置机器人的当前位置和目标位置。
机器人初始分配=路径(1,:);机器人目标=路径(结束,:);
假设一个初始机器人方向(机器人方向是机器人方向和正x轴之间的角度,逆时针测量)。
initialOrientation = 0;
定义机器人的当前姿势[x yθ]
robotCurrentPose=[RobotInitialOrientation];
初始化机器人模型并分配初始姿态。该仿真机器人具有两轮差动驱动机器人的运动方程。这个模拟机器人的输入是线速度和角速度。
机器人=运动学(“TrackWidth”, 1“车辆输入”,“VehicleSpeedHeadingRate”);
想象想要的路径
图形绘制(路径(:,1),路径(:,2),“k--d”) xlim([0 13])
根据上面定义的路径和机器人的运动模型,需要一个路径跟随控制器来驱动机器人沿路径前进。属性创建路径跟随控制器控制器追踪
对象
控制器=控制器追踪;
使用上面定义的路径为控制器设置所需的航路点
控制器。航路点=路径;
按照控制器参数设置路径。在本例中,所需的线速度设置为0.6米/秒。
controller.DesiredLinearVelocity=0.6;
最大角速度作为旋转速度的饱和极限,在本例中设置为2弧度/秒。
控制器。MaxAngularVelocity = 2;
一般来说,向前看距离应大于平滑路径所需的线速度。向前看距离较大时,机器人可能会抄近路。相反,较小的向前看距离可能导致不稳定的路径跟随行为。本例选择的值为0.3 m。
controller.lookaheaddance=0.3;
路径跟随控制器为机器人提供输入控制信号,机器人使用该信号沿所需路径驱动自身。
定义目标半径,该半径是机器人最终位置和目标位置之间所需的距离阈值。一旦机器人距离目标在这个距离内,它就会停止。此外,还可以计算机器人位置和目标位置之间的当前距离。根据目标半径持续检查该距离,当该距离小于目标半径时,机器人停止。
请注意,目标半径的值太小可能会导致机器人错过目标,这可能会导致目标附近出现意外行为。
goalRadius=0.1;distanceToGoal=norm(机器人初始分配-机器人目标);
的控制器追踪
对象计算机器人的控制命令。使用这些控制命令驱动机器人,直到到达目标半径范围内。如果您使用的是外部模拟器或物理机器人,则控制器输出应应用于机器人,并且可能需要定位系统来更新机器人的姿势。控制器以10赫兹的频率运行。
%初始化模拟循环sampleTime = 0.1;vizRate = rateControl (1 / sampleTime);初始化数字数字%确定车辆框架大小最接近的车辆与plot变换frameSize = robot.TrackWidth / 0.8;虽然(distanceToGoal > goalRadius)%计算控制器输出,即机器人的输入[v, omega] =控制器(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)];plotTransforms(plotTrVec',plotRot,“MeshFilePath”,“groundvehicle.stl”,“家长”甘氨胆酸,,“视图”,“二维”,“帧大小”, frameSize);淡定;xlim([0 13]) ylim([0 13]) waitfor(vizRate);结束
如果路径规划器计算所需的一组路径点,路径跟踪控制器可以以同样的方式使用。首先,可视化地图
负载示例地图映射=二进制职业映射(simpleMap);图显示(映射)
你可以计算路径
使用PRM路径规划算法。看到不同复杂度环境下的路径规划获取详细信息。
mapInflated = (map)复印件;充气(mapInflated, robot.TrackWidth / 2);人口、难民和移民事务局= robotics.PRM (mapInflated);人口、难民和移民事务局。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
显示充气地图、路线图和最终路径。
显示(人口、难民和移民事务局);
您定义了一个路径跟踪控制器,在该控制器之上,您可以重复使用该控制器来计算此地图上机器人的控制命令。要重新使用控制器并重新定义航路点,同时保持其他信息不变,请使用释放
函数。
释放(控制器);控制器。航路点=路径;
根据路径定义机器人的初始位置和目标
机器人初始分配=路径(1,:);机器人目标=路径(结束,:);
假设机器人的初始方向
initialOrientation = 0;
定义机器人运动的当前姿势[x y]
robotCurrentPose=[RobotInitialOrientation];
计算到目标位置的距离
distanceToGoal = norm(robotInitialLocation - robotGoal);
定义目标半径
目标半径=0.1;
使用给定地图上的控制器输出驱动机器人,直到它到达目标。控制器运行频率为10hz。
重置(利率);初始化数字数字虽然(distanceToGoal > goalRadius)%计算控制器输出,即机器人的输入[v, omega] =控制器(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)];plotTransforms(plotTrVec',plotRot,“MeshFilePath”,'groundvehicle.stl',“家长”甘氨胆酸,,“视图”,“二维”,“帧大小”,框架尺寸);光xlim([027])ylim([026])waitfor(vizRate);结束