轮式移动机器人轨迹跟踪的MATHLAB程序，运用运动学和动力学模型的双闭环控制，借鉴自抗扰控制技术结合了非线性ESO，跟踪效果良好，控制和抗扰效果较优，可分享控制结构图。 这段程序主要是一个小车的动力学仿真程序，用于模拟小车在参考轨迹下的运动。下面我将对程序进行详细的分析解释。 首先，程序开始时使用`clear`、`clc`和`close all`命令来清除工作空间、命令窗口和图形窗口中的内容。 接下来，程序定义了一系列参数和变量，用于设置仿真的参数和存储仿真过程中的数据。这些参数包括小车的质量、车宽、驱动轮半径等，还有参考轨迹的振幅和频率，仿真步长，仿真时间等。 然后，程序定义了一些元胞数组，用于存储不同阶段的数据。这些数组包括参考轨迹位姿、真实运动轨迹位姿、参考轨迹一阶导数、虚拟参考轨迹速度、期望速度、真实速度、控制器输出的控制力矩、控制输入、期望速度与真实速度误差、摩擦值、外界扰动值、总扰动、位姿跟踪误差、扰动观测值等。 接下来，程序给这些变量赋初始值，包括小车的初始位姿和速度，虚拟初始速度，期望初始速度，控制器输出的控制力矩，扰动观测值等。 然后，程序进入一个循环，仿真时间从0到给定的仿真时间tf。在每个循环中，程序根据给定的参考轨迹生成圆形参考轨迹，并计算参考轨迹的一阶导数和虚拟参考轨迹速度。 接下来，程序计算位姿误差，并使用运动学控制器计算期望速度。然后，程序使用非线性ESO（Extended State Observer）估计速度扰动，并使用动力学控制器计算控制力矩。最后，程序根据控制力矩和扰动计算小车的实际速度和位姿，并计算位姿跟踪误差。 在每个循环结束时，程序更新时间和循环索引，并继续下一次循环，直到仿真时间达到设定的结束时间tf。 最后，程序绘制了多个图形来展示仿真结果。这些图形包括参考轨迹和实际运动轨迹的图像、参考轨迹角度和实际角度的图像、内环速度误差的图像、位姿跟踪误差的图像、线速度扰动和观测器估计的图像、角速度扰动和观测器估计的图像、速度曲线的图像和控制量输入的图像。 通过这些图形，可以直观地了解小车在参考轨迹下的运动情况，以及控制器的性能和位姿跟踪误差等。 这段程序涉及到的知识点包括动力学方程、参考轨迹生成、运动学控制器、非线性ESO、动力学控制器等。它主要用于解决小车在给定参考轨迹下的运动控制问题，通过控制器输出的控制力矩来控制小车的速度和位姿，以实现对参考轨迹的跟踪。

一、代码整体概述

本MATLAB代码针对轮式移动机器人，构建了基于运动学与动力学模型的双闭环控制体系，并融入抗扰控制技术与非线性扩张状态观测器（ESO），实现了对圆形参考轨迹的精准跟踪。代码通过多模块协同工作，完成从参考轨迹生成、位姿误差计算、控制量输出到运动状态更新的全流程，同时具备对扰动的观测与抑制能力，最终通过多维度数据可视化呈现跟踪效果，为轮式移动机器人轨迹跟踪算法的验证与优化提供了完整的仿真平台。

二、核心功能模块解析

（一）仿真参数配置模块

该模块为整个仿真系统奠定基础，通过定义机器人物理属性、环境干扰参数、控制与观测器参数及时间参数，确保仿真场景与实际应用场景的一致性，具体功能如下：

1. 机器人物理属性参数：明确机器人关键物理特征，包括质量（m）、车宽（b）、驱动轮半径（r）、质心到驱动轮中点距离（d）及转动惯量（J）。这些参数直接决定机器人的动力学特性，例如质量和转动惯量会影响机器人对控制力矩的响应速度，驱动轮半径则关联线速度与角速度的转换关系。
2. 干扰参数：设置摩擦力（Fd）和外界扰动（tau_d），模拟机器人在实际运行中可能遇到的阻力与不确定性干扰，如地面摩擦阻力、风阻等，为后续抗扰控制提供干扰源。
3. 参考轨迹参数：定义圆形参考轨迹的振幅（A）和频率（omega），确定参考轨迹的形状与运动速度，振幅决定圆的半径，频率决定机器人沿轨迹运动的角速度。
4. 时间参数：设定仿真步长（h）、初始时间（to）和终止时间（tf）。仿真步长影响仿真精度与计算效率，步长越小精度越高但计算量越大；仿真时间范围则决定了轨迹跟踪效果的观测时长。
5. 控制与观测器参数：配置运动学控制器参数（ko、kt、kth）、观测器参数（w1、w2）及动力学控制器参数（kf）。这些参数直接影响控制性能，如运动学控制器参数决定位姿误差的收敛速度，观测器参数影响扰动观测的准确性，动力学控制器参数则关系到速度误差的抑制效果。
6. 矩阵参数：构建动力学模型中的关键矩阵Mba和Vmba。Mba矩阵反映机器人惯性特性，由质量和转动惯量等物理参数推导得出；Vmba矩阵根据模型化简结果设置，在本代码中简化为零矩阵，体现了对特定场景下机器人动力学模型的合理简化。

（二）变量与数据结构初始化模块

为保证仿真过程中数据的有序存储与调用，该模块定义了多种类型的变量与元胞数组，并完成初始值赋值，具体功能包括：

1. 变量类型与功能定义
   - 位姿相关变量：qr（参考轨迹位姿）、q（真实运动位姿），均包含x坐标、y坐标和转角（theta）三个维度，分别记录机器人期望运动状态与实际运动状态。
   - 速度相关变量：qrdot（参考轨迹一阶导数）、vr（虚拟参考轨迹速度）、Vqr（期望速度）、v（真实速度），涵盖线速度和角速度两个维度，用于描述机器人运动的快慢与转向变化。
   - 控制与误差变量：tauba（控制器输出控制力矩）、U（控制输入）、ec（期望速度与真实速度误差）、eq（位姿跟踪误差），实现控制量计算与跟踪精度评估。
   - 扰动与观测变量：Fba（摩擦值）、taudba（外界扰动值）、F_sum（总扰动）、X3（扰动观测值）、fal（非线性函数计算结果），支撑扰动的观测与抑制功能。
2. 元胞数组选择依据：考虑到仿真过程中各变量的维度会随时间步长动态变化，元胞数组具有灵活存储不同维度数据的特性，能够高效存储每一时间步的变量值，便于后续数据处理与结果分析。
3. 初始值赋值原则：参考轨迹初始位姿（q_r{1}(1:3,1)）与真实初始位姿（q{1}(1:3,1)）设置不同值，模拟机器人初始状态与期望状态的偏差；速度、控制量及扰动观测值的初始值根据实际工况经验设定，确保仿真启动时系统状态稳定，避免初始冲击。

（三）参考轨迹生成模块

该模块基于三角函数关系，在每一时间步生成圆形参考轨迹的位姿与速度信息，为机器人提供明确的运动目标，具体功能实现如下：

1. 位姿生成：利用余弦函数和正弦函数分别计算参考轨迹的x坐标（Acos(omegato)）和y坐标（Asin(omegato)），确保轨迹呈圆形；转角（omega*to+pi/2）随时间线性变化，保证机器人沿轨迹运动时的朝向与轨迹切线方向一致，符合轮式机器人运动特性。
2. 速度生成：对x坐标和y坐标分别求导，得到x方向和y方向的线速度分量，再通过平方和开方运算计算出合成线速度（sqrt((qrdot{1}(1,k))^2+(qrdot{1}(2,k))^2)）；转角的导数即为角速度（omega），为后续速度跟踪提供期望速度参考。

（四）位姿误差计算模块

为准确评估机器人实际位姿与参考位姿的偏差，该模块通过坐标系转换，将全局坐标系下的位姿误差转换为机器人本体坐标系下的误差，具体功能如下：

1. 误差转换原理：基于机器人当前转角（theta），利用三角函数构建坐标转换关系。x方向误差（eo）通过当前转角的余弦和正弦函数加权计算全局x、y坐标误差得到，y方向误差（et）通过负正弦和余弦函数加权计算全局x、y坐标误差得到，转角误差（e_th）直接为参考转角与真实转角的差值。
2. 转换意义：将全局误差转换为本体坐标系下的误差，更符合机器人的控制逻辑。因为机器人的控制量（如轮速）是基于本体坐标系输出的，本体坐标系下的误差能更直接地反映机器人在自身运动方向上的偏差，便于后续运动学控制器生成合理的期望速度。

（五）双闭环控制模块

作为代码的核心控制部分，双闭环控制模块包含运动学控制器（外环）和动力学控制器（内环），通过两层控制协同工作，实现高精度轨迹跟踪，具体功能如下：

1. 运动学控制器（外环）：以位姿误差为输入，输出期望速度。根据位姿误差（eo、et、eth）和虚拟参考轨迹速度（vr），结合预设的控制参数（ko、kt、kth），通过特定的控制算法计算期望线速度和角速度（Vq_r）。其核心作用是根据位姿偏差调整期望速度，使机器人逐步向参考轨迹靠拢，例如当x方向误差较大时，通过增大期望线速度中的误差补偿项，加快机器人在x方向的修正速度。
2. 动力学控制器（内环）：以速度误差为输入，输出控制力矩与控制输入。首先计算期望速度与真实速度的误差（ec），然后结合非线性ESO观测到的扰动信息（X2）和预设的控制器参数（Kf），计算出期望控制力矩（tauba）；再通过动力学模型中的惯性矩阵（Mba）求逆，将控制力矩转换为实际控制输入（U），作用于机器人驱动系统。该控制器能够快速抑制速度误差，确保机器人实际速度紧密跟踪期望速度，同时抵消扰动对速度的影响。

（六）非线性扩张状态观测器（ESO）模块

该模块是抗扰控制的关键，能够实时观测机器人运动过程中的未知扰动，并为控制器提供扰动补偿信息，具体功能实现如下：

1. 观测器核心逻辑：以机器人真实速度（v）与观测速度（X1）的误差（e_v）为输入，通过非线性fal函数处理误差信号，再结合控制输入（U）和预设的观测器参数（zo、zt），动态更新观测速度（X1）和扰动观测值（X2）。其中fal函数具有非线性特性，在误差较大时采用大增益快速跟踪误差，在误差较小时采用小增益保证观测精度，有效平衡观测速度与稳定性。
2. 扰动观测值计算：通过惯性矩阵（M_ba）与扰动观测中间值（X2）的乘积运算，得到最终的扰动观测值（X3），该值能够准确反映摩擦力、外界扰动等综合干扰的大小，为动力学控制器提供精准的扰动补偿依据，减少扰动对机器人运动状态的影响。

（七）运动状态更新模块

该模块根据动力学方程和运动学方程，结合控制输入与扰动信息，实时更新机器人的速度和位姿，模拟机器人的实际运动过程，具体功能如下：

1. 速度更新：基于动力学方程，考虑摩擦力（Fba）、外界扰动（taudba）和控制力矩（tauba）的综合作用，通过惯性矩阵（M_ba）求逆运算，计算出速度变化量，进而更新机器人的真实速度（v）。速度更新直接反映了控制输入对机器人运动状态的影响，以及扰动对速度的干扰程度。
2. 位姿更新：根据运动学方程，利用机器人当前转角构建转换矩阵（S），将速度信息转换为位姿变化量，从而更新机器人的真实位姿（q）。位姿更新结果是评估轨迹跟踪效果的直接依据，每一时间步的位姿数据构成了机器人的实际运动轨迹。

（八）结果可视化模块

该模块通过多组图形展示仿真结果，从不同维度直观呈现机器人轨迹跟踪性能、速度跟踪情况、扰动观测效果及控制输入特性，具体功能如下：

1. 轨迹对比图：绘制参考轨迹与实际轨迹，直观对比两者的重合程度，评估机器人在空间上的轨迹跟踪精度，可直接观察到实际轨迹是否紧密围绕参考轨迹运动。
2. 角度对比图：展示参考轨迹角度与实际跟踪角度随时间的变化曲线，分析机器人转角的跟踪效果，判断机器人在运动过程中朝向是否与参考轨迹要求一致。
3. 内环速度误差图：呈现内环线速度误差和角速度误差的变化趋势，反映动力学控制器对速度误差的抑制能力，误差曲线的收敛速度和波动范围是评估内环控制性能的关键指标。
4. 位姿跟踪误差图：显示x坐标、y坐标和转角的跟踪误差，全面评估机器人位姿跟踪的准确性，误差的大小和稳定性直接体现了整个控制体系的综合性能。
5. 扰动观测对比图：分别对比线速度扰动和角速度扰动的实际值与观测值，验证非线性ESO的观测精度，观测值与实际值的重合度越高，说明观测器的抗扰效果越好。
6. 速度曲线图：同时展示虚拟速度、期望速度与实际速度的变化情况，分析速度跟踪的动态过程，可观察到实际速度如何逐步逼近期望速度，以及虚拟速度对期望速度的引导作用。
7. 控制输入图：绘制两个控制输入随时间的变化曲线，反映控制器为实现轨迹跟踪所输出的控制量大小与变化规律，控制输入的平稳性和幅值范围为实际硬件设计提供参考。

三、代码运行流程

1. 初始化阶段：执行clear、clc、close all命令，清除工作空间变量、命令行窗口内容和已打开图形窗口，避免历史数据干扰；随后调用仿真参数配置模块和变量与数据结构初始化模块，完成参数设置与变量初始化。
2. 循环仿真阶段：以时间步长为单位，在仿真时间范围内（to < tf）循环执行以下操作：
   - 调用参考轨迹生成模块，生成当前时间步的参考位姿与参考速度。
   - 调用位姿误差计算模块，计算当前时间步的本体坐标系下位姿误差。
   - 调用运动学控制器，根据位姿误差和参考速度生成期望速度。
   - 调用非线性ESO模块，观测当前时间步的扰动信息。
   - 调用动力学控制器，根据速度误差和扰动观测值生成控制输入。
   - 调用运动状态更新模块，更新当前时间步的机器人真实速度与位姿。
   - 更新时间变量（to = to + h）和循环计数变量（k = k + 1），进入下一时间步循环。
3. 结果处理与可视化阶段：循环结束后，对仿真数据进行整理，计算时间轴数据（rk）；随后调用结果可视化模块，生成多组图形，展示仿真结果，为轨迹跟踪性能分析提供直观数据支持。

四、关键技术亮点

1. 双闭环控制架构：外环运动学控制确保机器人位姿向参考轨迹收敛，内环动力学控制快速抑制速度误差，两层控制相互配合，兼顾了轨迹跟踪的准确性和动态响应速度，有效提升了系统的控制精度与稳定性。
2. 非线性ESO抗扰技术：通过非线性fal函数实现对扰动的高精度观测，能够实时跟踪并补偿摩擦力、外界扰动等未知干扰，减少扰动对机器人运动状态的影响，增强了系统的鲁棒性，使机器人在复杂干扰环境下仍能保持良好的跟踪性能。
3. 坐标系转换误差计算：将全局坐标系下的位姿误差转换为本体坐标系下的误差，使误差信息更符合机器人控制逻辑，为运动学控制器生成合理的期望速度提供了准确依据，提高了误差修正的有效性。
4. 全面的结果可视化：通过多维度图形展示仿真结果，涵盖轨迹、角度、速度、误差、扰动及控制输入等关键指标，不仅便于直观评估控制性能，还能为算法优化提供清晰的数据支撑，例如通过扰动观测对比图可调整观测器参数以提高观测精度。

五、应用场景与扩展方向

1. 应用场景：本代码可广泛应用于轮式移动机器人轨迹跟踪算法的研发、验证与优化，如室内服务机器人、AGV（自动导引车）等需要精准轨迹跟踪的设备。在算法设计初期，可通过该代码快速验证控制算法的可行性；在算法优化阶段，可通过调整参数并观察仿真结果，优化控制性能；同时，也可作为教学案例，帮助相关专业学生理解轮式机器人运动控制原理与抗扰控制技术。
2. 扩展方向
   - 参考轨迹扩展：当前代码仅支持圆形参考轨迹，未来可扩展为直线、折线、椭圆等多种复杂轨迹，通过增加轨迹生成逻辑，满足不同应用场景下的轨迹跟踪需求。
   - 控制算法优化：可尝试引入先进的控制算法，如模型预测控制（MPC）、自适应控制等，与现有双闭环控制架构结合，进一步提升轨迹跟踪精度与系统鲁棒性。
   - 硬件在环仿真：将代码与实际机器人硬件结合，搭建硬件在环仿真平台，使仿真更贴近实际工况，为实际机器人控制程序的编写与调试提供更可靠的依据。
   - 多机器人协同控制：扩展代码功能，实现多台轮式移动机器人的协同轨迹跟踪，添加机器人间通信与避障逻辑，满足多机器人协作作业场景的需求。

轮式移动机器人轨迹跟踪的MATHLAB程序，运用运动学和动力学模型的双闭环控制，借鉴自抗扰控制技术结合了非线性ESO，跟踪效果良好，控制和抗扰效果较优，可分享控制结构图。 这段程序主要是一个小车的动力学仿真程序，用于模拟小车在参考轨迹下的运动。下面我将对程序进行详细的分析解释。 首先，程序开始时使用`clear`、`clc`和`close all`命令来清除工作空间、命令窗口和图形窗口中的内容。 接下来，程序定义了一系列参数和变量，用于设置仿真的参数和存储仿真过程中的数据。这些参数包括小车的质量、车宽、驱动轮半径等，还有参考轨迹的振幅和频率，仿真步长，仿真时间等。 然后，程序定义了一些元胞数组，用于存储不同阶段的数据。这些数组包括参考轨迹位姿、真实运动轨迹位姿、参考轨迹一阶导数、虚拟参考轨迹速度、期望速度、真实速度、控制器输出的控制力矩、控制输入、期望速度与真实速度误差、摩擦值、外界扰动值、总扰动、位姿跟踪误差、扰动观测值等。 接下来，程序给这些变量赋初始值，包括小车的初始位姿和速度，虚拟初始速度，期望初始速度，控制器输出的控制力矩，扰动观测值等。 然后，程序进入一个循环，仿真时间从0到给定的仿真时间tf。在每个循环中，程序根据给定的参考轨迹生成圆形参考轨迹，并计算参考轨迹的一阶导数和虚拟参考轨迹速度。 接下来，程序计算位姿误差，并使用运动学控制器计算期望速度。然后，程序使用非线性ESO（Extended State Observer）估计速度扰动，并使用动力学控制器计算控制力矩。最后，程序根据控制力矩和扰动计算小车的实际速度和位姿，并计算位姿跟踪误差。 在每个循环结束时，程序更新时间和循环索引，并继续下一次循环，直到仿真时间达到设定的结束时间tf。 最后，程序绘制了多个图形来展示仿真结果。这些图形包括参考轨迹和实际运动轨迹的图像、参考轨迹角度和实际角度的图像、内环速度误差的图像、位姿跟踪误差的图像、线速度扰动和观测器估计的图像、角速度扰动和观测器估计的图像、速度曲线的图像和控制量输入的图像。 通过这些图形，可以直观地了解小车在参考轨迹下的运动情况，以及控制器的性能和位姿跟踪误差等。 这段程序涉及到的知识点包括动力学方程、参考轨迹生成、运动学控制器、非线性ESO、动力学控制器等。它主要用于解决小车在给定参考轨迹下的运动控制问题，通过控制器输出的控制力矩来控制小车的速度和位姿，以实现对参考轨迹的跟踪。