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 = 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