차동구동로봇의경로추종

라이브스크립트열기

이예제는로봇시뮬레이터를사용하여로봇이목표경로를추종하도록제어하는방법을보여줍니다。이예제에서는纯粹追求경로추정제어기를사용해미리지정된경로를따라시뮬레이션로봇을구동합니다。목표경로란명시적으로정의되거나경로플래너를사용하여계산되는일련의웨이포인트입니다(不同复杂环境下的路径规划항목참조)。시뮬레이션차동구동로봇에대한경纯粹追求로추정제어기가만들어지고지정된경로를추종하도록제어명령을계산합니다。계산된제어명령을사용하여纯粹追求제어기를기반으로목표경로를추종하도록목표궤적을따라시뮬레이션로봇을구동합니다。

참고:R2016b부터는一步메서드를사용하여系统对象™로정의된작업을수행하는대신,마치함수인것처럼인수를사용하여객체를호출할수있습니다。예를 들어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]';

기구학적로봇모델만들기

로봇모델을초기화하고초기자세를할당합니다。시뮬레이션로봇은2륜차동구동로봇의모션에대한기구학방정식을가집니다。이시뮬레이션로봇에대한입력값은선속도와각속도입니다。

机器人=微分驱动器运动学(“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.3m로선택합니다。

控制器。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);结束