MATLAB+ROS Robot仿真（PurePursuit）類

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 = [e-mail](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
                [e-mail](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