www.gusucode.com > Mobile Robotics Simulation Toolbox > src/kinematics/FourWheelSteering.m
classdef FourWheelSteering < handle % FOURWHEELSTEERING Utilities for four-wheeled vehicle with front and % rear steering. % Ackermann steering is assumed, so the kinematic model is approximated % with a two-wheeled bicycle model with front and rear steering. % Source: https://www.ntu.edu.sg/home/edwwang/confpapers/wdwicar01.pdf % % For more information, see <a href="matlab:edit mrsDocFourWheelSteering">the documentation page</a> % % Copyright 2018 The MathWorks, Inc. properties wheelRadius = 0.1; % Wheel radius [m] frontWheelDist = 0.25; % CG to Front Wheel Distance [m] rearWheelDist = 0.25; % CG to Rear Wheel Distance [m] end properties (Access='private') tolerance = 1e-12; % Tolerance used to avoid divisions by zero end methods function obj = FourWheelSteering(wheelRadius,wheelDistances) % FOURWHEELSTEERING Construct an instance of this class % Unpack parameters obj.wheelRadius = wheelRadius; % Accepts either a 1 or 2-element array of wheel distances % In the 1-element case, front and rear wheel distances are % set to the value provided. if numel(wheelDistances) == 1 obj.frontWheelDist = wheelDistances; obj.rearWheelDist = wheelDistances; else obj.frontWheelDist = wheelDistances(1); obj.rearWheelDist = wheelDistances(2); end end function bodySpeeds = forwardKinematics(obj,wheelSpeeds,steerAngles) % Calculates linear and angular velocities [vx;vy;w], % in the *body* frame, from wheel speeds [wFront;wRear] and % steering angles [phiFront;phiRear] wFront = wheelSpeeds(1); wRear = wheelSpeeds(2); phiFront = steerAngles(1); phiRear = steerAngles(2); vx = 0.5*obj.wheelRadius*(wFront*cos(phiFront) + wRear*cos(phiRear)); vy = 0.5*obj.wheelRadius*(wFront*sin(phiFront) + wRear*sin(phiRear)); w = (wFront*sin(phiFront)-wRear*sin(phiRear))* ... obj.wheelRadius/(obj.frontWheelDist+obj.rearWheelDist); bodySpeeds = [vx;vy;w]; end function [wheelSpeeds,steerAngles] = inverseKinematicsFrontSteer(obj,vx,w) % Calculates wheel speeds [wFront;wRear] and steer angles % [phiFront;phiRear] from x velocity vx and angular velocity w % in the *body* frame. Assumes no rear steering and equal front % and rear wheel speeds. % Calculate front steer angle if abs(vx) > obj.tolerance % NOTE: Using atan instead of atan2 because we want steer % angle to be two-quadrant, in the range [-pi/2,pi/2] phiFront = atan(w*(obj.frontWheelDist+obj.rearWheelDist)/vx); else phiFront = 0; % Handle zero-speed case, which causes division by zero end % Calculate wheel speeds vRear = vx; % Rear wheel matches forward speed vFront = vx/cos(phiFront); % Front wheel matches forward speed given steer angles % Pack the outputs wheelSpeeds = [vFront;vRear]/obj.wheelRadius; steerAngles = [phiFront;0]; % Zero rear steering end function [wheelSpeeds,steerAngles] = inverseKinematicsZeroSideslip(obj,vx,w) % Calculates wheel speeds [wFront;wRear] and steer angles % [phiFront;phiRear] from x velocity vx and angular velocity w % in the *body* frame. Assumes equal front and rear wheel % speeds, as well as the front and rear steering angles being % opposite such that these is zero sideslip. % Calculate steer angle if abs(vx) > obj.tolerance % NOTE: Using atan instead of atan2 because we want steer % angle to be two-quadrant, in the range [-pi/2,pi/2] phi = atan(w*(obj.frontWheelDist+obj.rearWheelDist)/vx); else phi = 0; % Handle zero-speed case, which causes division by zero end % Calculate wheel speeds vWheel = vx/cos(phi); % Pack the outputs wheelSpeeds = [vWheel;vWheel]/obj.wheelRadius; % Equal wheel speeds steerAngles = [phi;-phi]; % Equal and opposite steer angles end function [wheelSpeeds,steerAngles] = inverseKinematicsParallelSteer(obj,vx,vy) % Calculates wheel speeds [wFront,wRear] and steer angles % [phiFront;phiRear] from linear velocities [vx;vy] % in the *body* frame. Assumes equal front and rear steering % angles so the vehicle moves linearly without rotating. % Calculate steer angle if abs(vx) > obj.tolerance % NOTE: Using atan instead of atan2 because we want steer % angle to be two-quadrant, in the range [-pi/2,pi/2] phi = atan(vy/vx); else % Set steer angle to +pi/2, -pi/2, or zero based on desired y speed phi = (pi/2)*sign(vy); end % Calculate wheel speeds % Turn wheels forward or backward based on X direction of speed vWheel = sqrt(vx^2 + vy^2)*sign(vx); % Pack the outputs wheelSpeeds = [vWheel;vWheel]/obj.wheelRadius; % Equal wheel speeds steerAngles = [phi;phi]; % Equal steer angles end end end