CN117706941A - 基于双舵轮移动机器人平移与旋转解耦的轨迹跟踪控制方法 - Google Patents

Abstract

本发明公开了一种基于双舵轮移动机器人平移与旋转解耦的轨迹跟踪控制方法，包括如下步骤：建立双舵轮移动机器人的运动学模型，并根据移动机器人的实际位姿和目标位姿建立位姿误差微分方程；利用反步法设计虚拟反馈变量和Lyapunov函数，计算Lyapunov函数关于控制系统的导数，并将位姿误差微分方程带入导数方程，设计出合适的控制率；分析控制率中参数对控制器性能的影响，设计2个三输入两输出的模糊控制器，根据车体运动状态动态调整相应的参数值；通过位姿解耦运动学转换成每个偏心舵轮的控制量：舵轮线速度v_k、舵轮角速度ω_i和舵轮转向角γ_i。通过所述方法可以实现移动机器人按照目标轨迹前进的同时任意旋转车体姿态，使其运动更加灵活。

Description

基于双舵轮移动机器人平移与旋转解耦的轨迹跟踪控制方法

技术领域

本发明涉及自动导引车技术领域，尤其涉及一种基于双舵轮移动机器人平移与旋转解耦的轨迹跟踪控制方法。

背景技术

AGV(Automated Guided Vehicle，自动导引车)是一种自动化程度高、效率高、安全性好的运输装置，广泛应用于现代物流仓储和制造业中。近年来，在日常文化发展过程中，舞台演艺文化逐渐占据重要地位，使用移动机器人将布景、道具、演员等根据演出剧场的变化按照预先设定的轨迹路线进行运动，以配合剧情演绎，丰富艺术展现力。而理论上以双舵轮为代表的移动机器人可以实现沿着指定轨迹前进的同时任意旋转车体姿态，但未见有相关研究和应用案例。这与传统移动机器人工作在工业运输场景下缺少相关需求有关。而当移动机器人应用于舞台表演等场合，前进的同时任意旋转车体姿态的特性能极大地增加节目观赏性，因此解决这一特性的控制难题变得尤为重要。

发明内容

本发明所要解决的技术问题是如何提供一种可以实现移动机器人按照目标轨迹前进的同时任意旋转车体姿态，运动更加灵活的基于双舵轮移动机器人平移与旋转解耦的轨迹跟踪控制方法。

为解决上述技术问题，本发明所采取的技术方案是：一种基于双舵轮移动机器人平移与旋转解耦的轨迹跟踪控制方法，包括如下步骤：

S1：建立双舵轮移动机器人的运动学模型，并根据移动机器人的实际位姿和目标位姿建立位姿误差微分方程；

S2：利用反步法设计虚拟反馈变量和Lyapunov函数，计算Lyapunov函数关于控制系统的导数，并将位姿误差微分方程带入导数方程，设计出合适的控制率；

S3：分析控制率中参数对控制器性能的影响，设计2个三输入两输出的模糊控制器，根据车体运动状态动态调整相应的参数值；

S4：通过位姿解耦运动学转换成每个偏心舵轮的控制量：舵轮线速度v_k、舵轮角速度ω_i和舵轮转向角γ_i。

采用上述技术方案所产生的有益效果在于：所述方法利用反步法和模糊控制推导得出，针对于安装两个舵轮的移动机器人，实现平移与旋转解耦并按目标轨迹运行，进而可以实现移动机器人按照目标轨迹前进的同时任意旋转车体姿态，运动更加灵活。

附图说明

下面结合附图和具体实施方式对本发明作进一步详细的说明。

图1为本发明实施例所述方法的流程图；

图2为本发明实例中移动机器人控制框图；

图3为本发明实例中对角安装的双舵轮移动机器人底盘的结构简图；

图4为本发明实例中双舵轮移动机器人自旋式前进的运动分解示意图；

图5为本发明实例中MATLAB/Simulink软件搭建的移动机器人反步法控制仿真模型图；

图6a-6d分别为本发明实例中利用反步法仿真所得到的横坐标误差、纵坐标误差、轨迹方向角误差和姿态角误差图·；

图7为本发明实例中MATLAB/Simulink软件搭建的移动机器人反步法模糊控制仿真模型图；

图8a-8d分别为本发明实例中利用反步法和模糊控制仿真所得到的横坐标误差、纵坐标误差、轨迹方向角误差和姿态角误差图。

具体实施方式

下面结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是本发明还可以采用其他不同于在此描述的其它方式来实施，本领域技术人员可以在不违背本发明内涵的情况下做类似推广，因此本发明不受下面公开的具体实施例的限制。

双舵轮以及多个舵轮的移动机器人的舵轮安装方式比较自由，可以安装在车体底盘的中线和对角线位置，通过调整每个舵轮的速度和角度，使移动机器人在车体姿态不变化的情况下实现万向平移和绕任意一点的转弯动作，通过本发明的算法可以实现移动机器人平移与旋转解耦并按目标轨迹运行，有更强的灵活性。本发明采用对角分布的双舵轮驱动的六轮移动机器人模型，另外四个轮子采用万向轮，以此模型进行说明。

总体的，如图1所示，本发明实施例公开了一种基于双舵轮移动机器人平移与旋转解耦的轨迹跟踪控制方法，包括如下步骤：

S1：建立双舵轮移动机器人的运动学模型，并根据移动机器人的实际位姿和目标位姿建立位姿误差微分方程；

S2：利用反步法设计虚拟反馈变量和Lyapunov函数，计算Lyapunov函数关于控制系统的导数，并将位姿误差微分方程带入导数方程，设计出合适的控制率；

S3：分析控制率中参数对控制器性能的影响，设计2个三输入两输出的模糊控制器，根据车体运动状态动态调整相应的参数值；

S4：通过位姿解耦运动学转换成每个偏心舵轮的控制量：舵轮线速度v_k、舵轮角速度ω_i和舵轮转向角γ_i。

图2是移动机器人控制框图，图中的目标位姿和目标运动状态是通过上层规划器给出，实际位姿和实际运动状态通过里程计和视觉相机组合定位计算得出。轨迹跟踪控制器利用反步法和模糊控制所设计，对移动机器人轨迹方向角和姿态角拆分处理。该控制器通过计算目标位姿和实际位姿的误差以及目标运动状态和实际运动状态的误差，输出合适的下一周期控制量：线速度轨迹方向角速度ω_β和姿态旋转角速度ω_α，再经过位姿解耦运动学转换成每个偏心舵轮的控制量：舵轮线速度v_k、舵轮角速度ω_i和舵轮转向角γ_i，对移动机器人运动状态进行调控，使其按照预设轨迹运行，从而实现轨迹跟踪的目的。

图3是对角安装的双舵轮移动机器人底盘的结构简图，可以表达出舵轮的安装位置，另外的四个支撑轮为万向轮。由于万向轮不影响移动机器人的运动方式，故在以后的图中不再画出万向轮

对双舵轮移动机器人建立位姿误差微分方程，图4为移动机器人轨迹跟踪误差图。图中q＝[x，y，β，α]^T为车体实际位姿，q_t＝[x_r，y_r，β_r，α_r]^T为车体目标位姿；e_x＝x_r-x和e_y＝y_r-y分别表示为车体目标位姿与实际位姿在全局参考坐标系下沿X轴和Y轴的误差分量；β_e为轨迹方向角误差分量，并规定β_e∈[0，2π]；α_e为姿态角误差分量，并规定α_e∈[0，2π]；同时对于车体目标位姿，其运动学模型为：

因实际测出的误差分量是在全局参考坐标系下，故需要转换到局部参考坐标系即车体坐标系来具体求解，系统位姿跟踪误差方程为：

系统位姿跟踪误差方程对时间求微分，可得到移动机器人跟踪误差微分方程为：

对移动机器人轨迹跟踪控制率设计。通过观察跟踪误差微分方程，对于x_e、y_e、β_e和α_e，可以发现误差变量y_e为间接控制量，当lim_t→∞x_e＝0和lim_t→∞β_e＝0时，此时为0，y_e即变为不可控量。无法确保使用x_e和β_e使得y_e收敛。下面将根据反步法构造虚拟反馈变量确保y_e先于x_e和β_e收敛，最终x_e、y_e和β_e三者均收敛于零。

对于移动机器人位姿误差微分方程式可知，当β_e→0时，可将x_e作为一个虚拟的控制输入，通过设计合适的虚拟状态反馈函数，使得y_e→0，这样对于自旋式AGV系统而言，利用反步法所设计控制器的控制目标为设计合适的速度向量u＝[v ω_β ω_α]^T，使得x_e趋近于所设计的虚拟状态反馈函数，且使得β_e→0。通过这种方法保证闭环系统的渐进稳定性。

根据反步法设计思想，针对误差分量x_e，构造虚拟反馈变量为当ω_β＝0时，k₁arctan(ω_β)y_e＝0。

根据跟踪误差微分方程，当lim_t→∞x_e→k₁arctan(ω_β)y_e且/>时，可得/> 基于此，对于误差分量y_e，设计部分Lyapunov函数：/>对时间求微分：/>当x_e→k₁arctan(ω_β)y_e且β_e→0时，可得：/>当且仅当ω_β＝0时，等号成立。当ω_β≠0时，/>为负定，/>为正定，则根据Lyapunov函数直接方法可知，当t→∞时，y_e渐进收敛于0。

对y_e和β_e三者，设计系统Lyapunov函数：

系统Lyapunov函数对时间求微分并带入相关方程整理可得：

选取如下控制率：

对α_e的控制，因α_e在轨迹误差微分方程中没有与其关联的项，对其控制率的设计满足其自身误差收敛即可，在这里为了方便计算，选取与控制β_e一样形式的控制率：

观察控制率中的第一个式子，可知控制输入关联于不可测的/>不可实现。故需要得到/>的表达式，/>的表达式为：/>

综上，可得到最终的控制率为：

图5为MATLAB/Simulink软件搭建的移动机器人反步法控制仿真模型图，A模块为移动机器人目标位姿模块，可以设定车体的目标运动状态和目标运动位姿；B模块为移动机器人运动学模块，可以设定车体的起始运动位姿，并接收新的控制量，通过运动学计算输出车体的实际运动位姿；C模块为移动机器人轨迹跟踪控制器模块，通过目标运动轨迹和目标运动状态，计算车体在运动轨迹上某点的位姿信息和运动状态信息。结合车体的当前实际位姿信息，计算车体的位姿误差信息。通过位姿误差信息来计算控制率，输出新的控制量，对移动机器人运动状态进行调整。

给定车体第一种运动状态：目标线速度为轨迹方向角速度为ω_β，r＝1rad/s和姿态角速度为ω_α，r＝1rad/s。第二种运动状态：目标线速度为/>轨迹方向角速度为ω_β，r＝1rad/s和姿态角速度为ω_α，r＝1rad/s。两种运动状态目标初始位姿为[0m 1m π/3 π/6]^T，实际初始位姿为[0.5m 1.5m 2π/3 π/2]^T，选择所设计的控制率参数为：k₁＝0.01、k₂＝5、k₃＝4、k₄＝16、k₅＝5、k₆＝16，采样时间为12ms。

图6a、图6b、图6c、图6d分别为横坐标误差图、纵坐标误差图、轨迹方向角误差图和姿态角误差图。通过仿真对比结果可以看出，当移动机器人速度在1m/s时能够很好的完成轨迹跟踪，收敛速度快且没有超调量；当速度在0.6m/s时，收敛速度变慢且有一定的超调量。在同一组控制率参数的作用下，不同的速度得出不同的结果，观察前文所设计的控制率，其不同的参数会影响控制率的效果。因此可以得出：基于反步法所设计的轨迹跟踪控制率中的参数对控制器的性能有很大的影响，当移动机器人在不同的工作场景中工作，其需要的速度各不相同，这种情况下，使用固定参数不易取得良好的控制效果。

根据控制率可知，x_e和y_e两种误差与速度控制有关，影响程度由k₁和k₃决定，理论上增大k₁和k₃的值可以提高x_e和y_e两种误差的收敛速度，但是由于移动机器人在实际运行过程中的加速度和系统延迟的问题，增大k₁和k₃的值对提高x_e和y_e两种误差收敛速度的效果不佳，而且过大的k₁和k₃会造成速度震荡，同时x_e和y_e两种误差较大时增大k₁和k₃会造成对线速度控制量偏大，从而使得对角度控制量偏小，这样会减慢α_e和β_e的收敛速度，因此将参数k₁和k₃的值设为固定值。

角度控制与y_e、β_e和α_e三种误差有关，影响程度由k₂、k₄、k₅和k₆，增大k₂、k₄、k₅和k₆可以提高y_e、β_e和α_e的收敛速度，但是k₂、k₄、k₅和k₆对线速度比较敏感，当线速度较大时，k₂和k₅过大会使得系统震荡，过小使得y_e收敛速度降低，同时k₂和k₄、k₅和k₆要成一定比例，使用模糊控制器根据移动机器人不同车速和误差大小对参数k₂、k₄、k₅和k₆的值自适应调节。

将目标车体速度、y_e误差和β_e误差作为模糊控制器的输入，将参数k₂和k₄作为模糊控制器的输出；将目标车体速度、y_e误差和α_e误差作为模糊控制器的输入，将参数k₅和k₆作为模糊控制器的输出。

输入输出的论域档级对模糊控制器的性能有很大影响，过多的档级会提高算法的复杂度，增加计算时间，降低控制的实时性，而过少的档级会降低模糊控制的准确度。根据实际运行情况，分别将速度论域[0m/s，2m/s]、y_e误差论域[-0.5m，0.5m]、β_e误差论域[-90°，90°]、α_e误差论域[-90°，90°]、参数k₂和k₅论域[1，50]以及参数k₄和k₆论域[10，40]分为7个论域，7个论域分别用{NB，NM，NS，ZE，PS，PM，PB}表示。同时根据各模糊变量确定其隶属度函数，选择三角形隶属度函数作为所模糊变量的隶属度函数。分别设置k₂k₄模糊控制器的输入量v、y_e、β_e以及输出量k₂、k₄的模糊论域、隶属度函数类型以及模糊子集分布。因控制率中，对β_e和α_e误差的控制率相同，故k₅k₆模糊控制器的输入量和输出量的模糊论域、隶属度函数类型以及模糊子集分布与k₂k₄模糊控制器相同，把输入量β_e、输出量k₂、k₄变换为输入量α_e、输出量k₅、k₆即可。

模糊推理规则是模糊控制器的核心，是决定控制器性能优劣的重要环节。根据上述分析，设计的模糊控制器推理规则的依据为：当车体速度较大时，为防止y_e误差对角度影响过大从而导致系统抖震，应输出较小的k₂、k₅，同时为了平衡y_e误差、β_e误差和α_e误差对角度控制的影响，需要输出较大的k₄、k₆；当y_e误差较大时，为了加快y_e误差的收敛速度，需要输出较大的k₂、k₅；当β_e误差和α_e误差较大时，为了加快β_e误差和α_e误差的收敛速度，需要输出较大的k₄、k₆。

图7为MATLAB/Simulink软件搭建的移动机器人反步法模糊控制仿真模型图。该仿真模型与上节仿真模型不同的是添加了2个三输入两输出的模糊控制器。模糊控制器可以根据车体的速度、纵向误差、轨迹方向角误差和姿态角误差通过模糊规则输出合适的参数值，将得到的参数值带入控制率中即可得到实时控制率，有效提高轨迹跟踪控制器的收敛速度和鲁棒性。

为了验证所设计的模糊轨迹跟踪控制器的性能，在不同速度下能够有很好的控制效果，给定与上文仿真实验相同车体的运动状态。第一种运动状态：目标线速度为第二种运动状态：目标线速度为/>并添加第三种运动状态：目标线速度为/>三种运动状态的轨迹方向角速度为ω_β，r＝1rad/s和姿态角速度为ω_α，r＝1rad/s；三种运动状态目标初始位姿为[0m 1m π/3 π/6]^T，实际初始位姿为[0.5m1.5m 2π/3 π/2]^T，采样时间为12ms。

图8a、图8b、图8c、图8d分别为横坐标误差图、纵坐标误差图、轨迹方向角误差图和姿态角误差图通过仿真对比结果可以看出，车体速度在0.6m/s、1m/s和1.5m/s的运动状态下，其横向误差、纵向误差、轨迹方向角误差和姿态角误差都可以在短时间内收敛到零，相比传统反步法设计固定参数的轨迹跟踪控制器，所设计的模糊控制器能够根据不同速度和误差大小输出合适的参数动态调节控制率，能够使移动机器人快速收敛到目标轨迹并稳定运行。

由轨迹跟踪控制器计算出的线速度轨迹方向角速度ω_β和姿态旋转角速度ω_α更新上位机在下一时刻车体位姿[x，y，β，α]在地面直角坐标系{W}下的运动轨迹{Path}：

以上值均为随时间变化的状态变量。

以车体为研究对象，求每个转轴中心点处的瞬时线速度。

因移动机器人自旋式前进，转轴处角速度不等于车体形心处角速度，即转轴中心点的运动轨迹与车体形心点的运动轨迹不同。

已知上位机给出车体形心点运动位置坐标随时间变化为：

车体姿态角随时间变化为：

α_(t)＝h(t)；

通过正交旋转矩阵和转移公式/>

将车体形心点的运动位置坐标转移到每个转轴中心点处，则每个转轴中心点在全局参考坐标系下的运动位置坐标随时间变化为：

上式中，(O_ix，O_iy)为两个转轴中心点在车体局部参考坐标系的位置坐标，当偏心舵轮安装在移动机器人时其转轴中心点位置坐标即为固定值。

对上式微分可得，每个转轴中心点线速度随时间的变化为：

以偏心舵轮为研究对象，求每个转轴中心点的角速度和转向角。

当移动机器人自旋式前进时，车体形心点处和转轴中心点处线速度方向均沿轨迹切线方向，故在此时刻所运行的半径为该点的曲率半径，曲率参数方程为：

因曲率参数方程其含有二阶导数，故对每个转轴中心点的运动轨迹式求二阶导数：

则每个转轴中心点的曲率半径为：

由每个转轴中心点的瞬时线速度和曲率半径，求得每个转轴中心点的角速度为：

因转轴中心点转向角＝转轴中心点轨迹方向角-车体姿态角，又因每个转轴中心点轨迹方向角为：

则每个转轴中心点转向角为：

已知每个转轴中心点线速度v_i、角速度ω_i、偏心值b，求每个舵轮线速度。

v_k＝v_i+ω_ib。

综上所述，以上计算过程是整套双舵轮移动机器人平移与旋转解耦的轨迹跟踪控制方法。

Claims (8)

1.一种基于双舵轮移动机器人平移与旋转解耦的轨迹跟踪控制方法，其特征在于包括如下步骤：

S1：建立双舵轮移动机器人的运动学模型，并根据移动机器人的实际位姿和目标位姿建立位姿误差微分方程；

S2：利用反步法设计虚拟反馈变量和Lyapunov函数，计算Lyapunov函数关于控制系统的导数，并将位姿误差微分方程带入导数方程，设计出合适的控制率；

S3：分析控制率中参数对控制器性能的影响，设计2个三输入两输出的模糊控制器，根据车体运动状态动态调整相应的参数值；

S4：通过位姿解耦运动学转换成每个偏心舵轮的控制量：舵轮线速度v_k、舵轮角速度ω_i和舵轮转向角γ_i。

2.如权利要求1所述的基于双舵轮移动机器人平移与旋转解耦的轨迹跟踪控制方法，其特征在于，所述建立位姿误差微分方程的方法包括如下步骤：

S1：由上位机发送给移动机器人车体目标位姿q_t＝[x_r，y_r，β_r，α_r]^T，其中x_r，y_r，β_r，α_r分别为移动机器人目标位姿的横坐标、纵坐标、轨迹方向角和姿态角；车体定位系统识别移动机器人车体实际位姿q＝[x，y，β，α]^T，其中x，y，β，α分别为移动机器人实际位姿的横坐标、纵坐标、轨迹方向角和姿态角；

S2：根据目标位姿和实际位姿得到系统位姿跟踪误差方程：

其中，e_x＝x_r-x和e_y＝y_r-y分别表示为车体目标位姿与实际位姿在全局参考坐标系下沿X轴和Y轴的误差分量；x_e和y_e分别为车体在局部参考坐标系下沿x轴和y轴的误差分量；β_e为轨迹方向角误差分量，并规定β_e∈[0，2π]；α_e为姿态角误差分量，并规定α_e∈[0，2π]；

S3：系统位姿跟踪误差方程对时间求微分，得到系统跟踪误差微分方程：

其中分别为x_e，y_e，β_e，α_e对时间求微分的值；/>分别为移动机器人车体形心目标线速度和实际线速度；ω_β，r，ω_β分别为移动机器人车体形心目标轨迹方向角速度和实际轨迹方向角速度；ω_α，r，ω_α分别为移动机器人车体形心目标姿态角速度和实际角速度。

3.如权利要求1所述的基于双舵轮移动机器人平移与旋转解耦的轨迹跟踪控制方法，其特征在于，所述控制率通过如下步骤进行实现：

S1：针对误差分量x_e，构造虚拟反馈变量为/> 当ω_β＝0时，k₁arctan(ω_β)y_e＝0；

S2：根据跟踪误差微分方程， 当lim_t→∞x_e→k₁arctan(ω_β)y_e且/>时，可得/>基于此，对于误差分量y_e，设计部分Lyapunov函数：/>对时间求微分：/> 当x_e→k₁arctan(ω_β)y_e且β_e→0时，可得：/> 当且仅当ω_β＝0时，等号成立；当ω_β≠0时，/>为负定，/>为正定，则根据Lyapunov函数直接方法可知，当t→∞时，y_e渐进收敛于0；

S3：对y_e和β_e三者，设计系统Lyapunov函数：

系统Lyapunov函数对时间求微分并带入相关方程整理可得：

选取如下控制率：

S4：对α_e的控制，因α_e在轨迹误差微分方程中没有与其关联的项，对其控制率的设计满足其自身误差收敛即可，选取与控制β_e一样形式的控制率：

观察控制率中的第一个式子，可知控制输入关联于不可测的/>不可实现；故需要得到/>的表达式，/>的表达式为：/>

综上，可得到最终的控制率为：

4.如权利要求1所述的基于双舵轮移动机器人平移与旋转解耦的轨迹跟踪控制方法，其特征在于，所述设计2个三输入两输出的模糊控制器的方法包括如下步骤：

S1：将目标车体速度在局部参考坐标系下车体形心沿Y轴的误差y_e和轨迹方向角误差β_e作为模糊控制器的输入，将参数k₂和参数k₄作为模糊控制器的输出设计模糊控制器；

S2：将目标车体速度在局部参考坐标系下车体形心沿Y轴的误差y_e和姿态角误差α_e作为模糊控制器的输入，将参数k₅和参数k₆作为模糊控制器的输出设计模糊控制器。

5.如权利要求1所述的基于双舵轮移动机器人平移与旋转解耦的轨迹跟踪控制方法，其特征在于，通过位姿解耦运动学转换成每个偏心舵轮的控制量的方法包括如下步骤：

S1：以双舵轮移动机器人车体为研究对象，求出偏心舵轮两个转轴中心点处的瞬时线速度v_i；

S2：以两个偏心舵轮为研究对象，求出两个偏心舵轮转轴中心点的轨迹方向角速度ω_i和转向角γ_i；

S3：根据所求出偏心舵轮两个转轴中心点处的瞬时线速度v_i、轨迹方向角速度ω_i和偏心值b，求出两个舵轮的线速度v_k。

6.如权利要求5所述的基于双舵轮移动机器人平移与旋转解耦的轨迹跟踪控制方法，其特征在于，瞬时线速度v_i通过如下方法获得：

上位机给出车体形心点运动位置坐标随时间变化为：

车体姿态角随时间变化为：α_(t)＝h(t)；

通过正交旋转矩阵和转移公式

将车体形心点的运动位置坐标转移到每个转轴中心点处，则每个转轴中心点在全局参考坐标系下的运动位置坐标随时间变化为：

上式中，(O_ix，O_iy)为两个转轴中心点在车体局部参考坐标系的位置坐标，当偏心舵轮安装在移动机器人时其转轴中心点位置坐标即为固定值；

对上式微分可得，每个转轴中心点线速度随时间的变化为：

7.如权利要求5所述的基于双舵轮移动机器人平移与旋转解耦的轨迹跟踪控制方法，其特征在于，以两个偏心舵轮为研究对象，求出两个偏心舵轮转轴中心点的轨迹方向角速度ω_i和转向角γ_i具体包括如下步骤：

当移动机器人自旋式前进时，车体形心点处和转轴中心点处线速度方向均沿轨迹切线方向，故在此时刻所运行的半径为该点的曲率半径，曲率参数方程为：

因曲率参数方程其含有二阶导数，故对每个转轴中心点的运动轨迹式求二阶导数：

则每个转轴中心点的曲率半径为：

由每个转轴中心点的瞬时线速度和曲率半径，求得每个转轴中心点的角速度为：

因转轴中心点转向角＝转轴中心点轨迹方向角-车体姿态角，又因每个转轴中心点轨迹方向角为：

则每个转轴中心点转向角为：

8.如权利要求5所述的基于双舵轮移动机器人平移与旋转解耦的轨迹跟踪控制方法，其特征在于，舵轮的线速度v_k通过如下公式计算：

v_k＝v_i+ω_ib。