导数
描述
stateDot= ' (motionModel,状态,cmds)stateDot作为一个三元素向量(xDot yDot thetaDot如果运动模型bicycleKinematics,differentialDriveKinematics,或unicycleKinematics对象。它返回状态作为一个研制出向量,xDot yDot thetaDot psiDot),如果是一个运动模型ackermannKinematics对象。xDot和yDot指定的车辆速度,指在米每秒。thetaDot车辆航向角速度和吗psiDot车辆转向的角速度,指定的弧度每秒。
例子
模拟不同的移动机器人运动学模型
这个例子展示了如何在一个环境模型不同的机器人运动学模型和比较。
定义移动机器人运动学约束
有许多方法来移动机器人的运动学模型。所有决定车轮速度有关机器人的状态:(x yθ),因为xy -坐标和一个机器人,θ在弧度。
独轮车运动模型
最简单的方法来表示车辆移动机器人运动学是独轮车模型,这一套车轮速度绕中心轴旋转,主对其z轴。差动式驱动和自行车运动模型减少了独轮车运动学输入时提供车辆速度和车辆航向率和其他约束不考虑。
独轮车= unicycleKinematics (“VehicleInputs”,“VehicleSpeedHeadingRate”);
差动传动运动学模型
差动驱动模型使用一个后置驱动轴控制车辆速度和速率。驱动轴上的轮子可以在两个方向旋转。因为大多数低级轮移动机器人有接口命令,这个模型将再次使用车辆的速度和航向率作为输入来简化车辆控制。
diffDrive = differentialDriveKinematics (“VehicleInputs”,“VehicleSpeedHeadingRate”);
区分独轮车的行为模型,添加一个车轮速度速度约束差动传动运动学模型
diffDrive。WheelSpeedRange =[10] -10 * 2 *π;
自行车运动模型
自行车模型轿车把机器人作为一个模型与两轴:后轮驱动轴和前轴,z轴。假设车轮下的自行车模型工作在每个轴可以建模为一个单一的、集中轮,而前轮标题可以直接设置,像一辆自行车。
自行车= bicycleKinematics (“VehicleInputs”,“VehicleSpeedHeadingRate”,“MaxSteeringAngle”π/ 8);
其他模型
阿克曼运动学模型假定阿克曼转向轿车修改模型。在大多数轮式车辆,前轮不相同的轴,而是打开稍微不同的轴,以确保他们乘坐同心圆的中心车辆的转弯。这种差异在转向角叫做阿克曼转向,和通常由一个机制在实际执行车辆。从车辆和车轮运动学的角度来看,它可以执行由治疗转向角速率输入。
汽车式样= ackermannKinematics;
设置仿真参数
这些移动机器人将遵循一组路径点,旨在表现出一些差异引起的不同的运动学。
路点= [0 0;0 10;10 10;5 10;11日9;4 5];%定义的总时间和采样率sampleTime = 0.05;%样本时间[s]tVec = 0: sampleTime: 20;%的时间数组:initPose =[锚点(1)';0);%初始姿势(x yθ)
创建一个车辆控制器
车辆跟随一组路径点使用一个纯粹追求控制器。给定一组的锚点,机器人当前状态,和其他一些参数,控制器输出车辆速度和航向。
%定义一个控制器。每个机器人都需要自己的控制器管理员一= controllerPurePursuit (“锚点”锚点,“DesiredLinearVelocity”3,“MaxAngularVelocity”,3 *π);controller2 = controllerPurePursuit (“锚点”锚点,“DesiredLinearVelocity”3,“MaxAngularVelocity”,3 *π);controller3 = controllerPurePursuit (“锚点”锚点,“DesiredLinearVelocity”3,“MaxAngularVelocity”,3 *π);
模拟模型使用ODE求解器进行求解
模型模拟使用导数函数来更新状态。这个例子使用一个常微分方程(ODE)解算器生成一个解决方案。另一种方法是使用一个循环更新状态,如图所示差动驱动机器人的路径跟踪。
自从ODE求解器要求所有输出作为一个单一的输出,提供纯粹追求控制器必须包装在一个函数,输出线速度和航向角速度作为一个单独的输出。助手一个例子,exampleHelperMobileRobotController用于这一目的。例子的助手也确保了机器人停止时指定的半径内的目标。
goalPoints =路标点(最终,:)”;goalRadius = 1;
数值为每种类型的模型被称为一次。函数的导数计算状态与初始状态设定的输出initPose。每一阶导数接受相应的运动学模型对象,当前的机器人位姿,控制器的输出构成。
%计算为每个运动轨迹运动控制模型[tUnicycle, unicyclePose] =数值(@ (t, y)导数(独轮车,y, exampleHelperMobileRobotController(管理员一y goalPoints goalRadius)), tVec, initPose);[tBicycle, bicyclePose] =数值(@ (t, y)导数(自行车,y, exampleHelperMobileRobotController (controller2 y goalPoints goalRadius)), tVec, initPose);[tDiffDrive, diffDrivePose] =数值(@ (t, y)导数(diffDrive y exampleHelperMobileRobotController (controller3 y goalPoints goalRadius)), tVec, initPose);
阴谋的结果
ODE求解器的结果可以很容易地在一个阴谋plotTransforms可视化的结果所有轨迹。
造成输出必须首先被转换成索引翻译和四元数的矩阵。
unicycleTranslations = [unicyclePose(: 1:2) 0(长度(unicyclePose), 1)];unicycleRot = axang2quat ([repmat((0 0 1),长度(unicyclePose), 1) unicyclePose (:, 3)));bicycleTranslations = [bicyclePose(: 1:2) 0(长度(bicyclePose), 1)];bicycleRot = axang2quat ([repmat((0 0 1),长度(bicyclePose), 1) bicyclePose (:, 3)));diffDriveTranslations = [diffDrivePose(: 1:2) 0(长度(diffDrivePose), 1)];diffDriveRot = axang2quat ([repmat((0 0 1),长度(diffDrivePose), 1) diffDrivePose (:, 3)));
接下来,所有变换的集合可以绘制和从顶部。独轮车的路径、自行车和差动式驱动机器人是红色,蓝色,和绿色。为了简化情节,只显示每十项输出。
图绘制(锚点(:1),锚点(:,2),“kx - - - - - -”,“MarkerSize”,20);持有所有plotTransforms (unicycleTranslations (1:10:,:), unicycleRot(1:10:最终,),“MeshFilePath”,“groundvehicle.stl”,“MeshColor”,“r”);plotTransforms (bicycleTranslations (1:10:,:), bicycleRot(1:10:最终,),“MeshFilePath”,“groundvehicle.stl”,“MeshColor”,“b”);plotTransforms (diffDriveTranslations (1:10:,:), diffDriveRot(1:10:最终,),“MeshFilePath”,“groundvehicle.stl”,“MeshColor”,“g”);视图(0,90)
模拟阿克曼与转向角约束运动模型
模拟一个移动机器人模型,使用阿克曼转向限制其转向角。模型在仿真过程中,保持最大转向角后到达转向限制。看到转向饱和的影响,你比较两个机器人的轨迹,与转向角上的约束和其他没有任何指导约束。
定义模型
定义了阿克曼运动学模型。轿车在这个模型中,前轮相距一定距离。以确保他们打开同心圆,车轮转向角度不同。转弯时,前轮接收指导输入转向角的变化速度。
汽车式样= ackermannKinematics;
设置仿真参数
设置移动机器人跟踪恒定线速度和接收一个常数转向率作为输入。模拟受限机器人长的一段演示转向饱和。
velo = 5;%恒定线速度psidot = 1;%常数离开转向率%定义的总时间和采样率sampleTime = 0.05;%样本时间[s]timeEnd1 = 1.5;%为无约束机器人仿真结束时间timeEnd2 = 10;%仿真结束时间约束的机器人tVec1 = 0: sampleTime: timeEnd1;%时间数组不受约束的机器人tVec2 = 0: sampleTime: timeEnd2;数组%时间约束的机器人initPose = (0, 0, 0, 0);%初始姿势(x yθ,φ)
创建选项结构ODE求解器进行求解
在本例中,您通过一个选项结构作为参数ODE求解器进行求解。的选项结构包含的信息转向角的限制。创建选项结构,使用事件选择odeset和创建的事件函数,detectSteeringSaturation。detectSteeringSaturation设置最大转向角为45度。
对于如何定义的描述detectSteeringSaturation,请参阅定义事件函数最后这个例子。
选择= odeset (“事件”,@detectSteeringSaturation);
使用ODE求解器模拟模型
接下来,您使用导数函数和一个ODE求解器,数值,解决模型和生成解决方案。
%模拟无约束的机器人(t1, pose1) =数值(@ (t, y)导数(汽车式样y [velo psidot]), tVec1, initPose);%模拟受限机器人(t2, pose2 te,你们()=数值(@ (t, y)导数(汽车式样y [velo psidot]), tVec2, initPose,选项);
检测转向饱和
当模型达到转向限制,注册一个事件的时间戳。的时间限制存储在te。
如果te < timeEnd2 str1 =“转向角限制了”;str2 =“秒”;comp = str1 + te + str2;disp (comp)结束
转向角限制了0.785秒
模拟受限机器人的新初始条件
现在使用受限机器人的状态之前终止集成作为第二个模拟的初始条件。修改输入向量来代表转向饱和,也就是说,将转向速度设置为零。
saturatedPsiDot = 0;%饱和度后转向率cmds = [velo saturatedPsiDot];%命令向量tVec3 = te: sampleTime: timeEnd2;%的时间向量pose3 = pose2(长度(pose2):);(t3、pose3 te3、ye3 ie3] =数值(@ (t, y)导数(汽车式样,y, cmds) tVec3, pose3,选项);
阴谋的结果
情节机器人的轨迹情节中存储的数据姿势。
图(1)情节(pose1 (: 1), pose1 (:, 2),“——r”,“线宽”2);持有在;情节([pose2 (: 1);pose3 (: 1)]、[pose2 (:, 2); pose3 (:, 2)),‘g’);标题(“轨迹x - y”)包含(“X”)ylabel (“Y”)传说(“无约束的机器人”,“限制机器人”,“位置”,“西北”)轴平等的
无约束的机器人是一个螺旋轨迹曲率半径减小而限制机器人跟随一个圆形轨迹后常曲率半径达到转向限制。
定义事件函数
设置事件函数集成4状态时终止,θ,等于最大转向角。
函数(状态、isterminal方向)= detectSteeringSaturation (t、y) maxSteerAngle = 0.785;%最大转向角(π/ 4弧度)状态(4)= (y (4) - maxSteerAngle);%饱和度事件发生在第四状态,θ,等于最大转向角isterminal (4) = 1;%集成事件发生时终止方向(4)= 0;%双向终止结束
输入参数
输出参数
引用
[1](merrill Lynch)、凯文·M。,和Frank C. Park.现代机器人:力学、规划和控制。第1版。剑桥,麻州:剑桥大学出版社,2017年。
扩展功能
版本历史
介绍了R2019b
