MATLAB+ROS Robot仿真（PurePursuit）类

﹏ヽ暗。殇╰゛Y 2022-01-29 05:55 288阅读 0赞

# PurePursuit类 #

### 文章目录 ###

*  PurePursuit类
 *   *  介绍
     *  语法
     *   *  创建对象
         *  step语法：
     *  PUREPURSUIT类内函数（method）：
     *  PUREPURSUIT属性：
     *  例程
     *  源码

## 介绍 ##

PUREPURSUIT 创建一个控制器用来控制机器人沿着一组航点运动。

PurePursuit 控制器是一个跟踪路径的几何控制器。给定一组航路点，PurePursuit 控制器为差分驱动机器人的给定姿态计算线速度和角速度控制输入，即PurePursuit 控制器计算的输出结果可以直接作为查分机器人的期望运动 。

## 语法 ##

### 创建对象 ###

PP = robotics.PUREPURSUIT

返回PurePursuit 系统对象PP，它使用PurePursuit算法计算差分驱动机器人的线速度和角速度输入。

PP = robotics.PUREPURSUIT('PropertyName'，PropertyValue，...)

返回PurePursuit对象PP，每个指定的属性设置为指定值。

### step语法： ###

[V，W] = step(PP，POSE)

使用PurePursuit算法求出3x1输入矢量POSE的线速度V和角速度W. POSE是机器人的当前位置，\[x y orientation\]。输出速度V和W可以应用于实际或模拟的差动驱动机器人，以沿着期望的路点运动。

[V，W，LOOKAHEADPOINT] = step(PP，POSE)

返回1x2数组LOOKAHEADPOINT，它是用于计算速度命令的路径上的\[x y\]位置。路径上的位置是基于LookaheadDistance属性计算的，因此需要在调用该函数之前初始化LookaheadDistance属性。

**备注**  
  航点可以包含NaN值。包含NaN值的行将被忽略。如果所有航点均为NaN，则返回零速度输出。

可以像函数一样直接调用系统对象，而不是step方法。例如

y = step(obj，x)

等价于

y = obj(x)

## PUREPURSUIT类内函数（method）： ##

*  step- 计算线性和角速度控制命令
 *  release - 允许属性值更改
 *  reset - 将内部状态重置为默认
 *  clone - 创建具有相同属性的纯追踪对象属性值％
 *  isLocked - 锁定状态（逻辑）
 *  info - 获取有关对象的其他信息

## PUREPURSUIT属性： ##

*  Waypoints - 表示要遵循的路径的航点
 *  MaxAngularVelocity - 所需的最大角速度
 *  LookaheadDistance - 计算控件的前瞻距离
 *  DesiredLinearVelocity - 所需的恒定线速度

## 例程 ##

%创建一个PurePursuit对象
    pp = robotics.PurePursuit;
    
    %分配一系列航点
    pp.Waypoints = [0 0; 1 1; 3 4];
    
    %计算初始姿态的控制输入[x y theta]
    [v，w] = pp([0 0 0]);
    
    %计算lookaheadPoint
    [v，w，lookaheadPoint] = pp([0 0 0]);

## 源码 ##

classdef (StrictDefaults)PurePursuit < robotics.algs.internal.PurePursuitBase
        properties
            %Waypoints The waypoints representing a path to follow
            %
            %   Default: []
            Waypoints
        end
        
        methods
            function set.Waypoints(obj, waypts)
                %set.Waypoints Setter for Waypoints property
                
                obj.validateWaypoints(waypts, 'PurePursuit', 'Waypoints');
                obj.Waypoints = waypts;
            end
            
            function obj = PurePursuit(varargin)
                %PurePursuit Constructor
                setProperties(obj,nargin,varargin{ :},'Waypoints', ...
                    'DesiredLinearVelocity', 'MaxAngularVelocity', ...
                    'LookaheadDistance');
            end
        end
        
        methods (Access = protected)
            function setupImpl(obj,curPose)
                obj.LookaheadPoint = zeros(1,2, 'like', curPose);
                obj.LastPose = zeros(1,3, 'like', curPose);
                obj.ProjectionPoint = nan(1,2, 'like', curPose);
                obj.ProjectionLineIndex = cast(0, 'like', curPose);
            end
            
            function validateInputsImpl(obj, curPose)
                %validateInputsImpl Validate inputs before setupImpl is called
                coder.internal.errorIf(isempty(obj.Waypoints), ...
                    'robotics:robotalgs:purepursuit:EmptyWaypoints');
                
                obj.validatePose(curPose, 'step', 'pose');
            end
            
            function [v, w, lookaheadPoint] = stepImpl(obj,curPose)
                %stepImpl Compute control commands
                
                currentPose = obj.validatePose(curPose, 'step', 'pose');
                [v, w, lookaheadPoint] = obj.stepInternal(currentPose, obj.Waypoints);
            end
            
            function num = getNumInputsImpl(~)
                %getNumInputsImpl return number of inputs
                
                % Input is current pose
                num = 1;
            end
            
            function num = getNumOutputsImpl(~)
                %getNumOutputsImpl return number of outputs
                
                % Output is linear velocity, angular velocity and look ahead
                % point.
                num = 3;
            end
            
            
            function flag = isInputSizeLockedImpl(~,~)
                flag = true;
            end
            
            
            function processTunedPropertiesImpl(obj)
                %processTunedPropertiesImpl Perform calculations if tunable
                % properties change between calls to steps
                
                % Detect waypoints change
                waypointsChange = isChangedProperty(obj, 'Waypoints');
                if waypointsChange
                    obj.ProjectionLineIndex = cast(0, 'like', obj.ProjectionLineIndex);
                end
            end
            
            function s = saveObjectImpl(obj)
                %saveObjectImpl Custom save implementation
                s = saveObjectImpl@matlab.System(obj);
                
                s.ProjectionPoint = obj.ProjectionPoint;
                s.LookaheadPoint = obj.LookaheadPoint;
                s.LastPose = obj.LastPose;
                s.ProjectionLineIndex = obj.ProjectionLineIndex;
            end
            
            function loadObjectImpl(obj, svObj, wasLocked)
                %loadObjectImpl Custom load implementation
                
                obj.LookaheadPoint = svObj.LookaheadPoint;
                obj.LastPose = svObj.LastPose;
                
                if ~isfield(svObj, 'ProjPointIdx')
                    % If object is saved using current release (i.e. 16b onwards)
                    
                    obj.ProjectionPoint = svObj.ProjectionPoint;
                    obj.ProjectionLineIndex = svObj.ProjectionLineIndex;
                elseif ~isempty(svObj.LastPose)
                    % Ensure 15a-16a release objects are loaded accurately
                    
                    % Find index of each waypoint
                    coder.varsize('tempIndex');
                    tempIndex = 1;
                    for i=1:size(svObj.Waypoints,1)
                        distToPt = sum((svObj.Path - svObj.Waypoints(i,:)).^2,2);
                        tempIndex = [tempIndex find(distToPt < sqrt(eps)).'];  %#ok<AGROW>
                    end
                    index = sort(unique(tempIndex));
                    higherIdx = index > svObj.ProjPointIdx;
                    obj.ProjectionLineIndex = max(find(higherIdx, 1)-1, 1);
                    obj.ProjectionPoint = svObj.Path(svObj.ProjPointIdx, :);
                    computeProjectionPoint(obj, svObj.LastPose, svObj.Waypoints);
                else
                    % Uninitialized 15a-16a objects
                    % Only double datatype was supported in 15a-16a
                    obj.ProjectionPoint = nan(1,2);
                    obj.LookaheadPoint = zeros(1,2);
                    obj.LastPose = zeros(1,3);
                    obj.ProjectionLineIndex = 0;
                end
                
                if isempty(svObj.Waypoints)
                    % Prevent warning for uninitialized object
                    return;
                else
                    % Call base class method
                    loadObjectImpl@matlab.System(obj,svObj,wasLocked);
                end
            end
            
            function resetImpl(obj)
                %resetImpl Reset the internal state to defaults
                
                % To preserve datatype, reset using multiplication
                obj.ProjectionPoint = nan*obj.ProjectionPoint;
                obj.LookaheadPoint = 0*obj.LookaheadPoint;
                obj.LastPose = 0*obj.LastPose;
                obj.ProjectionLineIndex = 0*obj.ProjectionLineIndex;
            end
        end
        
    end