单球驱动自平衡机器人：位姿解算与控制系统的深度剖析与创新实践

单球驱动自平衡机器人：位姿解算与控制系统的深度剖析与创新实践一、引言1.1研究背景与意义在科技飞速发展的当下，机器人技术已成为衡量一个国家科技创新和高端制造业水平的重要标志，广泛应用于工业生产、医疗服务、物流运输、家庭陪伴等诸多领域，深刻改变着人们的生活和工作方式。单球驱动自平衡机器人作为机器人领域的创新成果，凭借其独特的结构和出色的性能，展现出了巨大的研究价值和应用潜力。单球驱动自平衡机器人以单个球体作为驱动轮，在水平方向的两个自由度上等效于正交倒立摆模型，具有多变量、非线性、强耦合等复杂特性。这种特殊的结构赋予了它诸多传统机器人所不具备的优势。在工业领域，其小巧灵活的身形能够在狭窄、复杂的生产线上自由穿梭，执行精密的物料搬运、零件装配等任务，有效提升生产效率和质量。例如在电子芯片制造车间，单球驱动自平衡机器人可以精准地将微小的芯片搬运至指定位置，避免人工操作可能带来的误差和损坏。在服务领域，它能在人群密集的场所如商场、酒店、医院等，为人们提供引导、配送等贴心服务。像在酒店中，它可以自主导航，将物品快速准确地送到客人房间，提升服务效率和顾客满意度。在军事侦察和安防监控方面，其出色的机动性和隐蔽性使其能够深入危险区域，获取关键信息，为保障国家安全和社会稳定发挥重要作用。比如在城市反恐行动中，单球驱动自平衡机器人可以悄无声息地潜入危险区域，对恐怖分子的活动进行实时监测，为警方制定作战计划提供有力支持。然而，要充分发挥单球驱动自平衡机器人的优势，实现其在各领域的广泛应用，还面临着诸多技术挑战。其中，位姿解算与控制系统是关键核心技术，直接关系到机器人的运动精度、稳定性和可靠性。精确的位姿解算能够实时准确地获取机器人在空间中的位置和姿态信息，为后续的控制决策提供坚实依据。而高效稳定的控制系统则负责根据位姿解算结果，精准地调节机器人的运动，使其能够按照预定轨迹稳定运行，完成各种复杂任务。如果位姿解算不准确，机器人可能会出现定位偏差，导致无法准确执行任务；如果控制系统不稳定，机器人在运动过程中可能会出现晃动、失控等问题，严重影响其性能和安全性。因此，开展单球驱动自平衡机器人位姿解算与控制系统研究具有重要的现实意义，有助于突破技术瓶颈，推动机器人技术的发展，为各领域的智能化升级提供有力支持。1.2国内外研究现状近年来，单球驱动自平衡机器人在国内外都受到了广泛关注，众多学者和研究机构围绕其展开了深入研究，在结构设计、位姿解算算法、控制系统设计等方面取得了一定的进展。在结构设计方面，研究人员不断探索创新，提出了多种不同的设计方案，以提高机器人的稳定性和机动性。例如，传统的三足式驱动结构在一定程度上能够实现机器人的运动，但在复杂地形下的适应性和稳定性还有待提高。为了解决这一问题，一些研究设计了四足式单球驱动自平衡机器人，其底部驱动结构在空间上高度对称，有效降低了运动过程中的振动和冲击，提高了机器人的稳定性和灵活性。这种结构设计使得机器人在面对不平整地面或障碍物时，能够更加轻松地调整姿态，保持平衡并继续前行。在位姿解算算法方面，常用的方法包括基于传感器融合的算法和基于模型的算法。基于传感器融合的算法通过融合陀螺仪、加速度计、磁力计等多种传感器的数据，来提高位姿估计的精度和可靠性。例如，采用互补滤波算法，融合陀螺仪的高频特性和加速度计、磁力计的低频特性，能够有效抑制噪声干扰，实现对机器人姿态的准确估计。然而，这种算法在传感器精度有限或受到外界干扰时，位姿解算的准确性仍会受到影响。基于模型的算法则是通过建立机器人的运动学和动力学模型，利用模型预测和状态估计的方法来求解位姿。如扩展卡尔曼滤波（EKF）算法，能够根据机器人的运动模型和传感器测量值，对机器人的位姿进行实时估计和更新。但该算法对模型的准确性要求较高，模型误差可能会导致位姿解算结果出现偏差。在控制系统设计方面，研究人员尝试了多种控制策略，如PID控制、滑模控制、模糊控制等。PID控制是一种经典的控制方法，通过调整比例、积分、微分三个参数，对机器人的姿态和速度进行控制，具有结构简单、易于实现的优点。但对于具有强非线性和强耦合特性的单球驱动自平衡机器人，PID控制的参数整定较为困难，控制效果往往不尽如人意。滑模控制则是通过设计滑模面，使系统状态在滑模面上滑动，从而实现对系统的控制，具有较强的鲁棒性和抗干扰能力。然而，滑模控制存在抖振问题，可能会影响机器人的运动平稳性。模糊控制则是基于模糊逻辑，根据机器人的状态和输入信息，通过模糊推理和决策来调整控制量，能够较好地处理非线性和不确定性问题。但模糊控制的规则制定依赖于经验，缺乏系统性和自适应性。尽管国内外在单球驱动自平衡机器人的研究上取得了一定成果，但目前仍存在一些不足之处。现有位姿解算算法在复杂环境下的精度和可靠性还有待进一步提高，例如在磁场干扰较大或传感器故障的情况下，位姿解算结果可能会出现较大误差。现有控制系统的性能还有提升空间，在应对突发情况或复杂任务时，机器人的响应速度和控制精度还不能完全满足实际需求。此外，机器人的能耗问题、与外部环境的交互能力等方面也需要进一步研究和改进。1.3研究目标与创新点本研究旨在深入探究单球驱动自平衡机器人的位姿解算与控制技术，解决现有研究中存在的问题，提高机器人的性能和可靠性，推动其在更多领域的实际应用。具体研究目标如下：提高位姿解算精度：通过改进传感器融合算法和优化运动模型，提高机器人在复杂环境下的位姿解算精度，减少误差，确保机器人能够准确感知自身位置和姿态，为后续控制提供可靠依据。优化控制系统性能：设计一种高效、稳定的控制系统，结合先进的控制策略，如自适应控制、智能控制等，提高机器人的响应速度和控制精度，使其能够在不同工况下稳定运行，灵活应对各种复杂任务和突发情况。增强机器人的实用性：综合考虑机器人的结构设计、能耗管理、通信能力等因素，提高机器人的整体性能和实用性，降低成本，使其更易于在实际场景中部署和应用。本研究的创新点主要体现在以下几个方面：提出新型位姿解算算法：针对现有位姿解算算法在复杂环境下精度不足的问题，提出一种基于深度学习的位姿解算算法。该算法利用神经网络强大的学习能力，对大量传感器数据和环境信息进行学习和分析，能够自适应地调整位姿解算模型，提高在复杂环境下的解算精度。设计自适应智能控制系统：将自适应控制和智能控制相结合，设计一种自适应智能控制系统。该系统能够根据机器人的运行状态和环境变化，自动调整控制参数和控制策略，实现对机器人的精准控制，提高系统的鲁棒性和适应性。实现多机器人协作控制：探索单球驱动自平衡机器人的多机器人协作控制技术，通过设计合理的协作策略和通信协议，实现多机器人之间的协同工作，提高工作效率和完成复杂任务的能力，拓展机器人的应用场景。二、单球驱动自平衡机器人的结构与工作原理2.1机械结构设计单球驱动自平衡机器人的机械结构主要由球体、驱动装置、支撑结构等部分组成，各部分紧密协作，共同实现机器人的稳定运行和灵活移动。球体：作为机器人的驱动轮，球体通常采用高强度、轻量化的材料制成，如铝合金、碳纤维等。这些材料具有良好的强度和刚度，能够承受机器人在运动过程中所受到的各种力，同时又能减轻机器人的整体重量，提高能源利用效率。球体的表面通常经过特殊处理，以增加与地面之间的摩擦力，确保机器人在各种地形上都能稳定行驶。例如，采用橡胶材质的球体表面可以有效提高摩擦力，防止机器人在光滑地面上打滑；而采用带有纹理的表面处理方式，则可以进一步增强摩擦力，提高机器人在不平整地面上的通过性。驱动装置：驱动装置是机器人实现运动的核心部件，主要包括电机、减速器、驱动轮等。电机为机器人提供动力，常见的电机类型有直流电机、交流电机、步进电机等。直流电机具有响应速度快、控制精度高的优点，适合用于对运动精度要求较高的场合；交流电机则具有效率高、可靠性强的特点，适用于长时间运行的任务；步进电机则能够精确控制旋转角度和速度，常用于需要精确位置控制的应用中。减速器用于降低电机的转速，提高输出扭矩，使机器人能够获得足够的动力来驱动球体运动。驱动轮通常采用万向轮或全向轮，能够实现机器人在各个方向上的自由移动。例如，采用麦克纳姆轮作为驱动轮的机器人，可以通过控制四个轮子的转速和转向，实现全方位的移动，包括前进、后退、横向移动、旋转等。支撑结构：支撑结构用于支撑机器人的主体部分，确保机器人在运动过程中的稳定性。常见的支撑结构有三足式、四足式、框架式等。三足式支撑结构简单，成本低，但稳定性相对较差，适用于对稳定性要求不高的场合；四足式支撑结构在空间上高度对称，能够有效降低运动过程中的振动和冲击，提高机器人的稳定性和灵活性，适用于对稳定性要求较高的应用；框架式支撑结构则具有较高的强度和刚度，能够承受较大的负载，常用于大型机器人或需要在恶劣环境下工作的机器人。支撑结构与球体之间通常采用柔性连接方式，如弹簧、橡胶垫等，以减少振动和冲击对机器人的影响，提高机器人的运动平稳性。2.2工作原理概述单球驱动自平衡机器人的工作原理基于力学原理和运动控制原理，通过驱动球体实现自平衡和移动。从力学原理角度来看，机器人在运动过程中需要保持平衡，这就要求其重心始终位于支撑点的正上方。当机器人受到外界干扰或自身运动状态发生改变时，重心会偏离支撑点，从而产生一个使机器人倾倒的力矩。为了保持平衡，机器人需要通过驱动球体产生一个反向的力矩，来抵消倾倒力矩。根据牛顿第二定律，力等于质量乘以加速度，机器人通过控制驱动装置施加在球体上的力，来改变球体的运动状态，进而调整自身的姿态和位置，使重心重新回到支撑点的正上方。在运动控制原理方面，机器人主要依靠传感器实时获取自身的位姿信息，包括位置、姿态、速度等。常用的传感器有陀螺仪、加速度计、磁力计、编码器等。陀螺仪能够测量机器人的角速度，通过积分运算可以得到机器人的姿态角度；加速度计则用于测量机器人的加速度，结合陀螺仪的数据，可以更准确地计算出机器人的姿态和位置；磁力计可以提供机器人的航向信息，帮助机器人确定自身的方向；编码器则安装在驱动轮上，用于测量轮子的转速和转动角度，从而计算出机器人的移动速度和位移。控制器根据传感器采集到的位姿信息，结合预设的运动目标，通过特定的控制算法计算出驱动装置的控制量，如电机的转速、转向等。常见的控制算法有PID控制、滑模控制、模糊控制、自适应控制等。这些控制算法各有优缺点，在实际应用中需要根据机器人的特点和任务需求进行选择和优化。例如，PID控制算法结构简单、易于实现，适用于对控制精度要求不高的场合；滑模控制算法具有较强的鲁棒性和抗干扰能力，能够在复杂环境下保持较好的控制效果；模糊控制算法则能够处理非线性和不确定性问题，适用于对控制灵活性要求较高的应用。驱动装置根据控制器输出的控制量，调整球体的运动状态，实现机器人的自平衡和移动。当机器人需要向前移动时，控制器会控制驱动装置使球体向前滚动；当机器人需要转向时，控制器会通过调整驱动装置的输出，使球体在不同方向上产生不同的转速，从而实现转向。三、位姿解算方法研究3.1位姿解算原理位姿解算是机器人领域中的关键技术，它旨在实时准确地确定机器人在空间中的位置和姿态信息。在单球驱动自平衡机器人中，位姿解算的精度和可靠性直接影响其运动控制的准确性和稳定性，对于机器人完成各种复杂任务至关重要。位置的计算原理主要基于机器人的运动学模型和传感器测量数据。通过对驱动球体的转速、转向等参数的测量，结合机器人的运动学方程，可以推算出机器人在二维平面上的位移。假设机器人的驱动球体半径为r，驱动电机的转速为\omega，运动时间为t，则机器人在直线运动时的位移s可以通过公式s=r\omegat计算得出。在实际应用中，还需要考虑机器人的转向运动，通过对驱动球体不同方向的转速控制，实现机器人的转向，进而计算出其在平面上的坐标变化。姿态角的计算则涉及到多个坐标系之间的转换以及传感器数据的融合处理。在机器人的位姿解算中，常用的坐标系包括惯性坐标系、机体坐标系和世界坐标系。惯性坐标系是一个固定的参考坐标系，通常以地球为基准；机体坐标系则固定在机器人上，随着机器人的运动而变化；世界坐标系是根据具体应用场景定义的全局坐标系。这些坐标系之间的转换关系是姿态角计算的基础。以欧拉角为例，它是一种常用的姿态表示方法，包括滚转角（Roll）、俯仰角（Pitch）和偏航角（Yaw）。滚转角是绕机器人机体坐标系X轴的旋转角度，反映了机器人的左右倾斜程度；俯仰角是绕Y轴的旋转角度，表示机器人的前后倾斜状态；偏航角是绕Z轴的旋转角度，体现了机器人的航向变化。通过陀螺仪、加速度计等传感器测量机器人在机体坐标系下的角速度和加速度信息，利用坐标变换公式，可以将这些信息转换到惯性坐标系或世界坐标系中，从而计算出欧拉角。例如，根据陀螺仪测量的角速度\omega_x、\omega_y、\omega_z，通过积分运算可以得到角度的变化量，再结合初始姿态角，就可以实时更新机器人的姿态信息。在实际的位姿解算过程中，由于传感器测量存在误差，且机器人的运动受到多种因素的干扰，如地面摩擦力的变化、外界振动等，因此需要采用有效的算法对传感器数据进行融合和处理，以提高位姿解算的精度和可靠性。常用的算法包括互补滤波算法、卡尔曼滤波算法等，这些算法将在后续章节中详细介绍。3.2基于多传感器融合的位姿解算算法3.2.1传感器选型与数据采集在单球驱动自平衡机器人的位姿解算中，传感器的选型至关重要，它直接影响到数据采集的准确性和可靠性，进而决定了位姿解算的精度。常用的传感器包括陀螺仪、加速度计、磁力计等，它们各自具有独特的工作原理和性能特点，在机器人位姿解算中发挥着不可或缺的作用。陀螺仪主要用于测量物体的角速度，通过检测科里奥利力来确定物体的旋转运动。在单球驱动自平衡机器人中，陀螺仪能够实时感知机器人的姿态变化率，为姿态解算提供关键的角速度信息。例如，当机器人发生倾斜或转动时，陀螺仪可以快速检测到相应的角速度变化，并将其转换为电信号输出。其优点是响应速度快、测量精度高，能够准确捕捉到机器人的动态姿态变化；然而，陀螺仪也存在积分漂移的问题，随着时间的积累，测量误差会逐渐增大，导致姿态估计出现偏差。加速度计则是用于测量物体的加速度，通过检测质量块在惯性力作用下的位移来计算加速度值。在机器人位姿解算中，加速度计可以测量机器人在各个方向上的加速度，结合重力加速度的信息，可以计算出机器人的姿态角度。当机器人处于静止状态时，加速度计测量的重力加速度分量可以反映出机器人的倾斜角度。加速度计的优点是测量精度较高，在静态或低速运动情况下能够提供较为准确的姿态信息；但在机器人快速运动或受到外界振动干扰时，其测量结果容易受到噪声的影响，导致精度下降。磁力计主要用于测量地球磁场的强度和方向，通过检测磁场对磁性元件的作用来获取磁场信息。在机器人位姿解算中，磁力计可以提供机器人的航向信息，帮助确定机器人在水平面上的方向。例如，当机器人在不同方向上运动时，磁力计可以感知到地球磁场的变化，从而计算出机器人的偏航角。磁力计的优点是能够提供绝对的航向参考，不受机器人运动状态的影响；但它容易受到外界磁场干扰的影响，如附近的金属物体、电子设备等，导致测量结果出现偏差。为了采集到准确可靠的传感器数据，需要对传感器进行合理的安装和校准。在安装过程中，要确保传感器的坐标轴与机器人的机体坐标系一致，以保证测量数据的准确性。同时，还需要对传感器进行校准，消除传感器本身的误差和漂移。对于陀螺仪，通常需要进行零偏校准和灵敏度校准，以提高其测量精度；对于加速度计，需要进行重力校准和温度补偿，以消除重力加速度和温度变化对测量结果的影响；对于磁力计，需要进行硬铁校准和软铁校准，以补偿外界磁场干扰对测量结果的影响。在数据采集过程中，还需要对传感器数据进行预处理，以提高数据的质量。常见的预处理方法包括滤波、降噪等。通过采用低通滤波器、高通滤波器、带通滤波器等，可以去除传感器数据中的高频噪声和低频干扰，提高数据的稳定性和可靠性。采用卡尔曼滤波、均值滤波等算法，对传感器数据进行处理，进一步提高数据的准确性和精度。3.2.2互补滤波算法实现互补滤波算法是一种常用的多传感器融合算法，它通过融合陀螺仪和加速度计的优势，实现对机器人姿态的准确估计。该算法的基本原理是利用陀螺仪的高频特性和加速度计的低频特性，对两者的数据进行加权融合，从而得到更加准确的姿态信息。以基于比例－积分（PI）型互补滤波的姿态估计算法为例，该算法的实现过程如下：首先，初始化四元数姿态角。四元数是一种用于表示旋转的数学工具，它能够避免欧拉角表示中存在的万向节死锁问题，在姿态解算中具有广泛的应用。在初始化时，将四元数的各个分量设置为初始值，通常将实部设置为1，虚部设置为0。然后，根据陀螺仪测量的角速度信息，利用四元数微分方程对姿态角进行更新。陀螺仪能够提供高频的角速度测量值，通过对角速度进行积分，可以得到姿态角的变化量。根据四元数微分方程：\begin{align*}\dot{q}&=\frac{1}{2}q\otimes\omega\\\end{align*}其中，\dot{q}是四元数的导数，q是当前的四元数，\omega是陀螺仪测量的角速度，\otimes表示四元数乘法。通过对该方程进行离散化处理，可以得到姿态角的更新公式：\begin{align*}q_{k+1}&=q_{k}+\frac{1}{2}\Deltatq_{k}\otimes\omega_{k}\\\end{align*}其中，q_{k+1}是下一时刻的四元数，q_{k}是当前时刻的四元数，\Deltat是时间间隔，\omega_{k}是当前时刻陀螺仪测量的角速度。接着，根据加速度计测量的加速度信息，计算姿态角的误差。加速度计在静态或低速运动情况下能够提供较为准确的姿态信息，通过将加速度计测量的加速度与重力加速度进行比较，可以得到姿态角的误差。设加速度计测量的加速度为\vec{a}，重力加速度在机体坐标系下的分量为\vec{g}，则姿态角的误差可以表示为：\begin{align*}\vec{e}&=\vec{a}-\vec{g}\\\end{align*}然后，利用比例－积分（PI）控制器对姿态角误差进行处理，得到修正量。PI控制器是一种经典的控制算法，它通过对误差的比例和积分运算，产生控制量，以减小误差。在互补滤波算法中，PI控制器的输出作为姿态角的修正量，用于调整陀螺仪更新后的姿态角。设比例系数为K_p，积分系数为K_i，姿态角误差为\vec{e}，则修正量\vec{\delta}可以表示为：\begin{align*}\vec{\delta}&=K_p\vec{e}+K_i\int\vec{e}dt\\\end{align*}最后，将修正量与陀螺仪更新后的姿态角进行融合，得到最终的姿态估计值。融合的过程可以通过四元数乘法实现，即将修正量表示为四元数形式，然后与陀螺仪更新后的四元数相乘，得到最终的姿态四元数。设修正量表示为四元数q_{\delta}，陀螺仪更新后的四元数为q_{gyro}，则最终的姿态四元数q_{final}可以表示为：\begin{align*}q_{final}&=q_{\delta}\otimesq_{gyro}\\\end{align*}通过上述步骤，基于比例－积分（PI）型互补滤波的姿态估计算法能够有效地融合陀螺仪和加速度计的数据，实现对机器人姿态的高精度估计。在实际应用中，还需要根据机器人的具体情况和性能要求，合理调整比例系数K_p和积分系数K_i，以优化算法的性能。3.2.3算法性能分析为了全面评估基于多传感器融合的位姿解算算法的性能，本研究通过一系列实验，从精度、稳定性等多个关键方面进行深入分析，并与其他常见算法展开对比，以明确该算法的优势与不足。在精度评估实验中，将单球驱动自平衡机器人放置于一个标准的测试场地，场地内设置了多个精确的定位标识点。机器人按照预定的轨迹进行运动，同时利用高精度的光学测量设备（如激光跟踪仪）实时记录机器人的真实位姿信息，作为参考基准。在机器人运动过程中，通过位姿解算算法实时计算机器人的位姿，并与参考基准进行对比，计算出位姿误差。实验结果表明，基于多传感器融合的位姿解算算法能够实现较高的精度。在静态环境下，姿态角的估计误差能够控制在较小范围内，滚转角、俯仰角和偏航角的误差均在±0.5°以内，位置误差能够控制在±5mm以内。在动态环境下，当机器人以不同的速度和加速度运动时，姿态角误差在±1°以内，位置误差在±10mm以内。与传统的基于单一传感器的位姿解算算法相比，如仅使用陀螺仪或加速度计进行位姿解算，基于多传感器融合的算法精度有了显著提升，有效减少了传感器误差和外界干扰对解算结果的影响。稳定性是衡量位姿解算算法性能的另一个重要指标。为了测试算法的稳定性，在实验中对机器人施加各种外界干扰，如振动、磁场干扰等，观察位姿解算结果的波动情况。当机器人受到振动干扰时，基于多传感器融合的算法能够通过互补滤波等方式，有效地抑制振动对传感器数据的影响，保持位姿解算结果的相对稳定。在磁场干扰环境下，通过对磁力计数据的合理处理和融合，算法能够减少磁场干扰对航向估计的影响，确保机器人的航向估计误差在可接受范围内。与其他一些算法相比，如基于扩展卡尔曼滤波（EKF）的算法，在受到强干扰时，EKF算法可能会出现滤波发散的问题，导致位姿解算结果严重偏离真实值；而基于多传感器融合的算法具有更好的鲁棒性，能够在一定程度上抵抗外界干扰，保持位姿解算结果的稳定性。在实时性方面，基于多传感器融合的位姿解算算法采用了高效的数据处理和融合策略，能够在较短的时间内完成位姿解算。通过对算法运行时间的测试，在搭载普通处理器的机器人平台上，算法的平均解算时间能够控制在10ms以内，满足大多数实时应用场景的需求。与一些复杂的深度学习算法相比，虽然深度学习算法在理论上能够实现更高的精度，但由于其计算复杂度高，需要大量的计算资源和时间进行模型训练和推理，实时性较差，难以满足机器人实时运动控制的要求；而基于多传感器融合的算法则在保证一定精度的前提下，具有更好的实时性，更适合应用于实际的机器人系统中。通过对基于多传感器融合的位姿解算算法的性能分析，结果表明该算法在精度、稳定性和实时性等方面都具有较好的表现，能够满足单球驱动自平衡机器人在不同应用场景下的位姿解算需求，具有较高的应用价值和推广潜力。然而，该算法在面对极端复杂环境或传感器故障等特殊情况时，仍存在一定的局限性，需要进一步研究和改进。3.3基于组合导航算法的位置解算3.3.1INS/POS组合导航原理惯性导航系统（INS）和位置传感器（POS）组合导航是一种广泛应用于机器人定位的技术，它巧妙地融合了INS和POS的优势，有效提升了位置解算的准确性和可靠性，为单球驱动自平衡机器人在复杂环境中的精确定位提供了有力支持。INS是一种基于牛顿力学原理的自主式导航系统，它主要由陀螺仪和加速度计组成。陀螺仪能够精确测量机器人的角速度，通过对角速度进行积分运算，可以得到机器人的姿态角变化，从而实时跟踪机器人的姿态变化情况。加速度计则用于测量机器人的加速度，结合姿态角信息，通过二次积分运算，可以推算出机器人的位移和速度。INS的显著优点是具有高度的自主性，不受外界环境干扰的影响，能够在任何时间、任何地点为机器人提供连续的导航信息。由于陀螺仪和加速度计存在测量误差，这些误差会随着时间的推移而逐渐累积，导致INS的定位误差随时间不断增大，长时间运行后定位精度会严重下降。POS则是一类用于直接测量机器人位置的传感器，常见的包括全球定位系统（GPS）、激光雷达、视觉相机等。以GPS为例，它通过接收卫星发射的信号，利用三角测量原理来确定机器人在地球上的地理位置。GPS具有定位精度高、覆盖范围广的优点，能够为机器人提供绝对位置信息。然而，GPS信号容易受到遮挡、干扰等因素的影响，在室内、城市峡谷等环境中，信号可能会减弱或中断，导致定位失效。激光雷达通过发射激光束并测量反射光的时间来获取周围环境的距离信息，从而构建地图并实现定位。但激光雷达价格昂贵，对环境要求较高，在恶劣天气条件下性能会受到影响。视觉相机则通过对拍摄的图像进行分析和处理，识别特征点并计算其位置，进而实现定位。视觉相机成本较低，但计算复杂度高，对光照条件敏感。INS/POS组合导航的核心原理是利用两者的优势互补，实现更准确的位置解算。在组合导航系统中，INS提供高频率的姿态和速度信息，能够实时跟踪机器人的动态变化；POS则提供高精度的位置信息，用于修正INS的累积误差。通过数据融合算法，将INS和POS的数据进行有机结合，相互校正和补充，从而提高位置解算的精度和可靠性。常见的数据融合算法有卡尔曼滤波、粒子滤波等。以卡尔曼滤波为例，它是一种基于线性最小均方误差估计的最优滤波算法，通过建立系统的状态方程和观测方程，对INS和POS的数据进行融合处理。卡尔曼滤波将系统的状态分为预测和更新两个阶段。在预测阶段，根据INS的运动模型和上一时刻的状态估计值，预测当前时刻的状态；在更新阶段，利用POS的测量值对预测结果进行修正，得到更准确的状态估计值。通过不断地预测和更新，卡尔曼滤波能够有效地融合INS和POS的数据，实现对机器人位置的精确解算。通过INS/POS组合导航，单球驱动自平衡机器人能够在复杂多变的环境中实现高精度的位置解算，为其后续的运动控制和任务执行提供可靠的位置信息，极大地拓展了机器人的应用范围和性能表现。3.3.2带观测量延时修正的卡尔曼滤波器设计在单球驱动自平衡机器人的位置解算中，卡尔曼滤波器作为一种常用的数据融合算法，能够有效地融合惯性导航系统（INS）和位置传感器（POS）的数据，提高位置解算的精度。然而，在实际应用中，由于传感器数据传输、处理等环节的影响，观测量往往存在一定的延时，这会对卡尔曼滤波器的性能产生负面影响，导致位置解算精度下降。为了解决这一问题，需要设计一种带观测量延时修正的卡尔曼滤波器。以某实际案例为例，在一个室内物流配送场景中，单球驱动自平衡机器人需要在仓库中准确地定位并搬运货物。机器人配备了INS和激光雷达作为POS，激光雷达通过扫描周围环境获取位置信息。由于激光雷达的数据处理和传输过程存在一定的时间延迟，导致其测量的位置信息不能及时反馈给卡尔曼滤波器，从而影响了位置解算的精度。为了修正观测量延时对解算精度的影响，首先需要对观测量延时进行准确的估计。可以通过实验测试或理论分析的方法，获取传感器数据传输和处理的时间延迟。在上述案例中，通过多次实验测量，确定激光雷达的观测量延时为T_d。然后，在卡尔曼滤波器的设计中，对状态方程和观测方程进行相应的修正。传统的卡尔曼滤波器状态方程为：[\begin{align*}\\begin{align*}\\四、控制系统设计与实现4.1控制系统架构单球驱动自平衡机器人的控制系统架构是一个复杂而精密的体系，它融合了先进的硬件和软件技术，旨在实现机器人的精确控制和稳定运行。该架构主要由硬件架构和软件架构两大部分组成，二者相互协作、紧密配合，共同确保机器人能够高效地完成各种任务。硬件架构是控制系统的物理基础，它主要包括传感器、处理器、驱动电路和通信模块等核心组件。传感器作为机器人的感知器官，负责实时采集机器人的位姿、速度、加速度等关键信息。陀螺仪能够精确测量机器人的角速度，为姿态控制提供重要数据；加速度计则用于检测机器人的加速度，辅助姿态解算和运动控制；磁力计可提供机器人的航向信息，帮助其确定自身方向；编码器安装在驱动轮上，用于测量轮子的转速和转动角度，从而计算出机器人的移动速度和位移。这些传感器采集的数据通过相应的接口传输给处理器，为后续的控制决策提供依据。处理器是整个控制系统的核心大脑，它承担着数据处理、算法运算和控制指令生成的重任。在本研究中，选用了高性能的STM32F407微控制器作为主处理器。该处理器具有强大的运算能力和丰富的外设资源，能够快速处理传感器传来的大量数据，并根据预设的控制算法计算出驱动装置的控制量。其高达168MHz的工作频率和192KB的SRAM，为复杂的控制算法实现和数据存储提供了有力支持。处理器通过通信接口与传感器、驱动电路和其他外部设备进行数据交互，确保整个系统的信息流通顺畅。驱动电路是连接处理器和电机的关键桥梁，它负责将处理器输出的控制信号转换为电机所需的驱动电流，从而控制电机的转速和转向。在本设计中，采用了基于H桥的电机驱动电路，能够实现电机的正反转和速度调节。H桥电路由四个功率开关管组成，通过控制开关管的导通和截止，可以改变电机两端的电压极性和大小，进而实现对电机的精确控制。为了提高驱动电路的效率和可靠性，还采用了功率放大器和保护电路，对驱动信号进行放大和保护，防止电机过载和短路对电路造成损坏。通信模块则用于实现机器人与上位机或其他设备之间的无线数据传输。在实际应用中，机器人可能需要接收上位机发送的任务指令和环境信息，同时也需要将自身的状态和运行数据反馈给上位机。本研究选用了Wi-Fi模块作为通信设备，它具有传输速度快、覆盖范围广的优点，能够满足机器人与上位机之间实时、高效的数据通信需求。通过Wi-Fi模块，机器人可以与远程控制中心进行通信，实现远程监控和控制，拓展了机器人的应用范围。软件架构是控制系统的灵魂，它主要包括控制算法、实时操作系统和应用程序等部分。控制算法是软件架构的核心，它根据传感器采集的数据和预设的控制目标，计算出驱动装置的控制量，以实现机器人的自平衡和运动控制。在本研究中，采用了先进的自抗扰控制（ADRC）算法，该算法能够有效地抑制系统中的干扰和不确定性，提高机器人的控制精度和鲁棒性。ADRC算法通过扩展状态观测器（ESO）对系统的状态和总扰动进行实时估计，并在控制律中对扰动进行补偿，从而实现对机器人的精确控制。实时操作系统（RTOS）在软件架构中起着至关重要的作用，它负责管理系统的任务调度、资源分配和中断处理等工作，确保系统能够在严格的时间约束内完成任务。在本设计中，选用了FreeRTOS作为实时操作系统，它具有开源、轻量级、可裁剪等优点，非常适合应用于嵌入式系统中。FreeRTOS提供了丰富的任务管理、信号量、消息队列等功能，能够方便地实现多任务并发执行和任务间的通信与同步。通过合理的任务划分和调度，实时操作系统能够确保控制算法、传感器数据采集、通信等任务的高效执行，提高系统的实时性和稳定性。应用程序则是用户与机器人进行交互的接口，它负责实现用户界面、任务规划和数据显示等功能。用户可以通过应用程序向机器人发送各种指令，如启动、停止、移动、转向等，同时也可以实时查看机器人的状态和运行数据。应用程序通常采用图形化界面设计，操作简单直观，方便用户使用。在本研究中，基于Qt开发框架设计了一个用户友好的应用程序，通过Wi-Fi与机器人进行通信，实现对机器人的远程控制和状态监测。硬件架构和软件架构相互协作，共同构成了单球驱动自平衡机器人的控制系统。硬件架构为软件架构提供了运行平台和数据采集、执行的基础，软件架构则通过控制算法和实时操作系统实现对硬件的精确控制和任务管理，二者缺一不可。只有合理设计和优化控制系统架构，才能充分发挥机器人的性能优势，实现其在各种复杂环境下的稳定运行和高效工作。4.2硬件系统设计4.2.1处理器选型与电路设计处理器作为单球驱动自平衡机器人控制系统的核心部件，其性能直接影响着机器人的整体性能和控制精度。在处理器选型过程中，需要综合考虑多个因素，以确保所选处理器能够满足机器人控制系统的需求。运算能力是处理器选型的关键因素之一。单球驱动自平衡机器人在运行过程中，需要实时处理大量的传感器数据，如陀螺仪、加速度计、磁力计等传感器采集的位姿、速度、加速度等信息。同时，还需要运行复杂的控制算法，如自抗扰控制（ADRC）算法、卡尔曼滤波算法等，以实现对机器人的精确控制。因此，要求处理器具有强大的运算能力，能够快速处理这些数据和算法。以STM32F407微控制器为例，其采用了Cortex-M4内核，工作频率高达168MHz，具备单周期乘法和硬件除法指令，能够快速执行各种数学运算，满足机器人控制系统对运算能力的要求。外设资源的丰富程度也是处理器选型的重要考虑因素。机器人控制系统需要与多种外部设备进行通信和交互，如传感器、驱动电路、通信模块等。因此，处理器需要具备丰富的外设接口，如SPI、I2C、UART、USB等，以实现与这些设备的连接和数据传输。STM32F407微控制器集成了多个SPI接口、I2C接口、UART接口和USB接口，能够方便地与各种传感器和执行器进行通信，满足机器人控制系统对外设资源的需求。功耗和成本也是处理器选型时需要考虑的重要因素。单球驱动自平衡机器人通常采用电池供电，因此要求处理器具有较低的功耗，以延长机器人的续航时间。同时，为了降低机器人的制造成本，也需要选择价格合理的处理器。STM32F407微控制器在保证高性能的同时，采用了低功耗设计，其工作电流在不同模式下均可控制在较低水平，且价格相对较为合理，在满足性能要求的同时，兼顾了功耗和成本因素。基于以上因素的综合考虑，本研究选用了STM32F407微控制器作为单球驱动自平衡机器人的主处理器。围绕该处理器进行的电路设计主要包括电源电路、时钟电路、复位电路等。电源电路为处理器和其他硬件设备提供稳定的工作电源。由于机器人通常采用电池供电，电源电路需要具备高效的电源转换和稳压功能。在本设计中，采用了LM2596降压芯片将电池的高电压转换为处理器所需的3.3V和1.8V工作电压。LM2596是一款开关型降压稳压器，具有高效率、高输出电流和良好的线性调整率等优点。通过合理设计外围电路，能够确保电源输出的稳定性和可靠性，为处理器和其他硬件设备提供稳定的电源供应。时钟电路为处理器提供精确的时钟信号，确保处理器能够按照预定的频率运行。STM32F407微控制器支持多种时钟源，包括高速外部时钟（HSE）、低速外部时钟（LSE）、高速内部时钟（HSI）和低速内部时钟（LSI）等。在本设计中，采用了8MHz的外部晶体振荡器作为HSE时钟源，经过PLL锁相环倍频后，为处理器提供168MHz的系统时钟。通过精确的时钟设置，能够保证处理器的运行速度和稳定性，提高系统的性能。复位电路用于在系统启动或异常情况下，将处理器和其他硬件设备恢复到初始状态。在本设计中，采用了简单的RC复位电路，通过电阻和电容的组合，在系统上电时产生一个短暂的低电平信号，使处理器进入复位状态。当系统正常运行后，复位信号保持高电平，确保处理器正常工作。同时，还可以通过手动复位按钮，在需要时强制系统复位，提高系统的可靠性和可维护性。通过合理的处理器选型和电路设计，能够为单球驱动自平衡机器人的控制系统提供稳定、高效的硬件平台，为机器人的精确控制和稳定运行奠定坚实的基础。4.2.2传感器接口设计传感器接口设计是单球驱动自平衡机器人硬件系统设计的重要环节，它直接关系到传感器数据能否准确、快速地传输到处理器，进而影响机器人的位姿解算和控制精度。在本研究中，主要涉及陀螺仪、加速度计、磁力计等传感器与处理器之间的接口设计。陀螺仪和加速度计通常采用SPI（SerialPeripheralInterface）或I2C（Inter-IntegratedCircuit）接口与处理器进行通信。SPI接口是一种高速、全双工的同步串行通信接口，具有数据传输速率快、通信协议简单等优点。以MPU6050传感器为例，它集成了陀螺仪和加速度计，通过SPI接口与STM32F407处理器连接。在SPI通信中，处理器作为主设备，传感器作为从设备。处理器通过SPI控制器发送时钟信号（SCK）、片选信号（CS）、数据输出信号（MOSI）和数据输入信号（MISO）。SCK信号用于同步数据传输，CS信号用于选择从设备，MOSI信号用于主设备向从设备发送数据，MISO信号用于从设备向主设备返回数据。通过这些信号的协同工作，处理器可以快速读取MPU6050传感器采集的角速度和加速度数据。I2C接口则是一种半双工的同步串行通信接口，具有接口简单、占用引脚少等优点。一些传感器，如LSM6DS3，既支持SPI接口，也支持I2C接口。在I2C通信中，处理器和传感器通过两根线进行通信，分别是串行数据线（SDA）和串行时钟线（SCL）。SDA用于传输数据，SCL用于同步数据传输。每个I2C设备都有一个唯一的地址，处理器通过发送设备地址来选择与之通信的传感器。在进行数据传输时，先发送起始信号，然后发送设备地址和读写命令，接着传输数据，最后发送停止信号。I2C接口适用于对数据传输速率要求不高，但对接口简单性和引脚资源有限的应用场景。磁力计常用于提供机器人的航向信息，常见的磁力计如HMC5883L通常采用I2C接口与处理器连接。在I2C通信过程中，磁力计作为从设备，处理器作为主设备。处理器通过I2C总线向磁力计发送控制指令，如设置测量模式、数据输出速率等，然后读取磁力计测量的磁场强度数据。通过对磁场强度数据的处理和分析，可以计算出机器人的航向角度。在传感器接口设计过程中，还需要考虑数据传输的稳定性和抗干扰能力。为了提高数据传输的稳定性，通常会在传感器与处理器之间添加滤波电路，以去除传输过程中的噪声干扰。采用低通滤波器可以滤除高频噪声，采用去耦电容可以减少电源噪声对信号的影响。合理布局电路板，减少信号传输线的长度和交叉，也有助于提高数据传输的稳定性和抗干扰能力。此外，为了确保传感器数据的准确性，还需要对传感器进行校准。不同类型的传感器校准方法各不相同。对于陀螺仪和加速度计，通常需要进行零偏校准和灵敏度校准。零偏校准是通过测量传感器在静止状态下的输出，计算出零偏值，并在后续的数据处理中进行补偿。灵敏度校准则是通过对传感器施加已知的角速度或加速度，测量其输出，计算出灵敏度系数，以提高测量精度。磁力计的校准则需要考虑地球磁场的影响，通过在不同方向上测量磁场强度，进行硬铁校准和软铁校准，以补偿外界磁场干扰对测量结果的影响。通过合理设计传感器接口，采用合适的通信协议和抗干扰措施，并对传感器进行准确校准，可以确保传感器数据的准确、快速传输，为单球驱动自平衡机器人的位姿解算和控制提供可靠的数据支持。4.3软件系统设计4.3.1控制算法实现控制算法是单球驱动自平衡机器人软件系统的核心，它直接决定了机器人的运动性能和控制精度。在本研究中，采用了基于自抗扰控制（ADRC）的姿态控制算法，以实现对机器人姿态的精确控制。以下通过具体代码示例展示该控制算法的实现过程。//定义ADRC控制器参数floatbeta01=100;//扩展状态观测器增益1floatbeta02=1000;//扩展状态观测器增益2floatbeta1=0.1;//控制律增益floatbeta2=1;//控制律增益floatbeta3=10;//控制律增益floattd0=0.01;//滤波时间常数//定义状态变量floatx1=0;//角度floatx2=0;//角速度floatx3=0;//总扰动估计值//扩展状态观测器（ESO）函数voideso(floaty,floatu,floatdt){floate=y-x1;x1=x1+dt*(x2-beta01*e);x2=x2+dt*(x3-beta02*e+u);x3=x3-dt*beta02*e;}//控制律计算函数floatcontrol_law(floatr,floatdt){floate1=r-x1;floate2=-x2;floatu0=beta1*e1+beta2*e2;floatu=u0-x3;returnu;}//主控制函数floatadrc_control(floatr,floaty,floatu_prev,floatdt){eso(y,u_prev,dt);floatu=control_law(r,dt);//限幅处理，防止控制量过大if(u>100){u=100;}elseif(u<-100){u=-100;}returnu;}floatbeta01=100;//扩展状态观测器增益1floatbeta02=1000;//扩展状态观测器增益2floatbeta1=0.1;//控制律增益floatbeta2=1;//控制律增益floatbeta3=10;//控制律增益floattd0=0.01;//滤波时间常数//定义状态变量floatx1=0;//角度floatx2=0;//角速度floatx3=0;//总扰动估计值//扩展状态观测器（ESO）函数voideso(floaty,floatu,floatdt){floate=y-x1;x1=x1+dt*(x2-beta01*e);x2=x2+dt*(x3-beta02*e+u);x3=x3-dt*beta02*e;}//控制律计算函数floatcontrol_law(floatr,floatdt){floate1=r-x1;floate2=-x2;floatu0=beta1*e1+beta2*e2;floatu=u0-x3;returnu;}//主控制函数floatadrc_control(floatr,floaty,floatu_prev,floatdt){eso(y,u_prev,dt);floatu=control_law(r,dt);//限幅处理，防止控制量过大if(u>100){u=100;}elseif(u<-100){u=-100;}returnu;}floatbeta02=1000;//扩展状态观测器增益2floatbeta1=0.1;//控制律增益floatbeta2=1;//控制律增益floatbeta3=10;//控制律增益floattd0=0.01;//滤波时间常数//定义状态变量floatx1=0;//角度floatx2=0;//角速度floatx3=0;//总扰动估计值//扩展状态观测器（ESO）函数voideso(floaty,floatu,floatdt){floate=y-x1;x1=x1+dt*(x2-beta01*e);x2=x2+dt*(x3-beta02*e+u);x3=x3-dt*beta02*e;}//控制律计算函数floatcontrol_law(floatr,floatdt){floate1=r-x1;floate2=-x2;floatu0=beta1*e1+beta2*e2;floatu=u0-x3;returnu;}//主控制函数floatadrc_control(floatr,floaty,floatu_prev,floatdt){eso(y,u_prev,dt);floatu=control_law(r,dt);//限幅处理，防止控制量过大if(u>100){u=100;}elseif(u<-100){u=-100;}returnu;}floatbeta1=0.1;//控制律增益floatbeta2=1;//控制律增益floatbeta3=10;//控制律增益floattd0=0.01;//滤波时间常数//定义状态变量floatx1=0;//角度floatx2=0;//角速度floatx3=0;//总扰动估计值//扩展状态观测器（ESO）函数voideso(floaty,floatu,floatdt){floate=y-x1;x1=x1+dt*(x2-beta01*e);x2=x2+dt*(x3-beta02*e+u);x3=x3-dt*beta02*e;}//控制律计算函数floatcontrol_law(floatr,floatdt){floate1=r-x1;floate2=-x2;floatu0=beta1*e1+beta2*e2;floatu=u0-x3;returnu;}//主控制函数floatadrc_control(floatr,floaty,floatu_prev,floatdt){eso(y,u_prev,dt);floatu=control_law(r,dt);//限幅处理，防止控制量过大if(u>100){u=100;}elseif(u<-100){u=-100;}returnu;}floatbeta2=1;//控制律增益floatbeta3=10;//控制律增益floattd0=0.01;//滤波时间常数//定义状态变量floatx1=0;//角度floatx2=0;//角速度floatx3=0;//总扰动估计值//扩展状态观测器（ESO）函数voideso(floaty,floatu,floatdt){floate=y-x1;x1=x1+dt*(x2-beta01*e);x2=x2+dt*(x3-beta02*e+u);x3=x3-dt*beta02*e;}//控制律计算函数floatcontrol_law(floatr,floatdt){floate1=r-x1;floate2=-x2;floatu0=beta1*e1+beta2*e2;floatu=u0-x3;returnu;}//主控制函数floatadrc_control(floatr,floaty,floatu_prev,floatdt){eso(y,u_prev,dt);floatu=control_law(r,dt);//限幅处理，防止控制量过大if(u>100){u=100;}elseif(u<-100){u=-100;}returnu;}floatbeta3=10;//控制律增益floattd0=0.01;//滤波时间常数//定义状态变量floatx1=0;//角度floatx2=0;//角速度floatx3=0;//总扰动估计值//扩展状态观测器（ESO）函数voideso(floaty,floatu,floatdt){floate=y-x1;x1=x1+dt*(x2-beta01*e);x2=x2+dt*(x3-beta02*e+u);x3=x3-dt*beta02*e;}//控制律计算函数floatcontrol_law(floatr,floatdt){floate1=r-x1;floate2=-x2;floatu0=beta1*e1+beta2*e2;floatu=u0-x3;returnu;}//主控制函数floatadrc_control(floatr,floaty,floatu_prev,floatdt){eso(y,u_prev,dt);floatu=control_law(r,dt);//限幅处理，防止控制量过大if(u>100){u=100;}elseif(u<-100){u=-100;}returnu;}floattd0=0.01;//滤波时间常数//定义状态变量floatx1=0;//角度floatx2=0;//角速度floatx3=0;//总扰动估计值//扩展状态观测器（ESO）函数voideso(floaty,floatu,floatdt){floate=y-x1;x1=x1+dt*(x2-beta01*e);x2=x2+dt*(x3-beta02*e+u);x3=x3-dt*beta02*e;}//控制律计算函数floatcontrol_law(floatr,floatdt){floate1=r-x1;floate2=-x2;floatu0=beta1*e1+beta2*e2;floatu=u0-x3;returnu;}//主控制函数floatadrc_control(floatr,floaty,floatu_prev,floatdt){eso(y,u_prev,dt);floatu=control_law(r,dt);//限幅处理，防止控制量过大if(u>100){u=100;}elseif(u<-100){u=-100;}returnu;}//定义状态变量floatx1=0;//角度floatx2=0;//角速度floatx3=0;//总扰动估计值//扩展状态观测器（ESO）函数voideso(floaty,floatu,floatdt){floate=y-x1;x1=x1+dt*(x2-beta01*e);x2=x2+dt*(x3-beta02*e+u);x3=x3-dt*beta02*e;}//控制律计算函数floatcontrol_law(floatr,floatdt){floate1=r-x1;floate2=-x2;floatu0=beta1*e1+beta2*e2;floatu=u0-x3;returnu;}//主控制函数floatadrc_control(floatr,floaty,floatu_prev,floatdt){eso(y,u_prev,dt);floatu=control_law(r,dt);//限幅处理，防止控制量过大if(u>100){u=100;}elseif(u<-100){u=-100;}returnu;}floatx1=0;//角度floatx2=0;//角速度floatx3=0;//总扰动估计值//扩展状态观测器（ESO）函数voideso(floaty,floatu,floatdt){floate=y-x1;x1=x1+dt*(x2-beta01*e);x2=x2+dt*(x3-beta02*e+u);x3=x3-dt*beta02*e;}//控制律计算函数floatcontrol_law(floatr,floatdt){floate1=r-x1;floate2=-x2;floatu0=beta1*e1+beta2*e2;floatu=u0-x3;returnu;}//主控制函数floatadrc_control(floatr,floaty,floatu_prev,floatdt){eso(y,u_prev,dt);floatu=control_law(r,dt);//限幅处理，防止控制量过大if(u>100){u=100;}elseif(u<-100){u=-100;}returnu;}floatx2=0;//角速度floatx3=0;//总扰动估计值//扩展状态观测器（ESO）函数voideso(floaty,floatu,floatdt){floate=y-x1;x1=x1+dt*(x2-beta01*e);x2=x2+dt*(x3-beta02*e+u);x3=x3-dt*beta02*e;}//控制律计算函数floatcontrol_law(floatr,floatdt){floate1=r-x1;floate2=-x2;floatu0=beta1*e1+beta2*e2;floatu=u0-x3;returnu;}//主控制函数floatadrc_control(floatr,floaty,floatu_prev,floatdt){eso(y,u_prev,dt);floatu=control_law(r,dt);//限幅处理，防止控制量过大if(u>100){u=100;}elseif(u<-100){u=-100;}returnu;}floatx3=0;//总扰动估计值//扩展状态观测器（ESO）函数voideso(floaty,floatu,floatdt){floate=y-x1;x1=x1+dt*(x2-beta01*e);x2=x2+dt*(x3-beta02*e+u);x3=x3-dt*beta02*e;}//控制律计算函数floatcontrol_law(floatr,floatdt){floate1=r-x1;floate2=-x2;floatu0=beta1*e1+beta2*e2;floatu=u0-x3;returnu;}//主控制函数floatadrc_control(floatr,floaty,floatu_prev,floatdt){eso(y,u_prev,dt);floatu=control_law(r,dt);//限幅处理，防止控制量过大if(u>100){u=100;}elseif(u<-100){u=-100;}returnu;}//扩展状态观测器（ESO）函数voideso(floaty,floatu,floatdt){floate=y-x1;x1=x1+dt*(x2-beta01*e);x2=x2+dt*(x3-beta02*e+u);x3=x3-dt*beta02*e;}//控制律计算函数floatcontrol_law(floatr,floatdt){floate1=r-x1;floate2=-x2;floatu0=beta1*e1+beta2*e2;floatu=u0-x3;returnu;}//主控制函数floatadrc_control(floatr,floaty,floatu_prev,floatdt){eso(y,u_prev,dt);floatu=control_law(r,dt);//限幅处理，防止控制量过大if(u>100){u=100;}elseif(u<-100){u=-100;}returnu;}voideso(floaty,floatu,floatdt){floate=y-x1;x1=x1+dt*(x2-beta01*e);x2=x2+dt*(x3-beta02*e+u);x3=x3-dt*beta02*e;}//控制律计算函数floatcontrol_law(floatr,floatdt){floate1=r-x1;floate2=-x2;floatu0=beta1*e1+beta2*e2;floatu=u0-x3;returnu;}//主控制函数floatadrc_control(floatr,floaty,floatu_prev,floatdt){eso(y,u_prev,dt);floatu=control_law(r,dt);//限幅处理，防止控制量过大if(u>100){u=100;}elseif(u<-100){u=-100;}returnu;}floate=y-x1;x1=x1+dt*(x2-beta01*e);x2=x2+dt*(x3-beta02*e+u);x3=x3-dt*beta02*e;}//控制律计算函数floatcontrol_law(floatr,floatdt){floate1=r-x1;floate2=-x2;floatu0=beta1*e1+beta2*e2;floatu=u0-x3;returnu;}//主控制函数floatadrc_control(floatr,floaty,floatu_prev,floatdt){eso(y,u_prev,dt);floatu=control_law(r,dt);//限幅处理，防止控制量过大if(u>100){u=100;}elseif(u<-100){u=-100;}returnu;}x1=x1+dt*(x2-beta01*e);x2=x2+dt*(x3-beta02*e+u);x3=x3-dt*beta02*e;}//控制律计算函数floatcontrol_law(floatr,floatdt){floate1=r-x1;floate2=-x2;floatu0=beta1*e1+beta2*e2;floatu=u0-x3;returnu;}//主控制函数floatadrc_control(floatr,floaty,floatu_prev,floatdt){eso(y,u_prev,dt);floatu=control_law(r,dt);//限幅处理，防止控制量过大if(u>100){u=