CN116991076B - 一种基于状态估计信息输入的轮式机器人转向控制方法 - Google Patents

Abstract

本发明公开了一种基于状态估计信息输入的轮式机器人转向控制方法，根据轮式机器人的运动特性，构建理想二自由度模型及状态空间方程，并计算得到理想横摆角速度，再建立平方根无迹卡尔曼滤波器，并获得轮式机器人的横摆角速度估计值和质心侧偏角估计值，构建自适应超螺旋滑模控制器，计算输出轮式机器人的主动前轮转角，进行轮式机器人的主动前轮转向控制。本发明以轮式机器人实时反馈的行驶参数(前轮转角和侧向加速度)为实际输入量，利用系统输入参数、轮式机器人结构参数、实时测量参数，结合状态信息估计与主动前轮转向自适应调整轮式机器人控制状态，能够充分利用轮式机器人的机动性，提高了路径跟踪过程的行驶稳定性。

Description

一种基于状态估计信息输入的轮式机器人转向控制方法

技术领域

本发明涉及轮式机器人运动控制领域，尤其涉及一种基于状态估计信息输入的轮式机器人转向控制方法。

背景技术

近年来，轮式机器人（又叫轮式移动机器人）已被广泛应用于许多领域，如物流配送、智慧社区、餐厅服务等。作为轮式机器人应用中最重要的问题之一，轮式机器人的状态参数估计和主动前轮转向控制引起了人们的广泛关注。

从状态估计角度讲，轮式机器人状态估计方法主要包括卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波、粒子滤波等，其中，卡尔曼滤波广泛应用于线性模型，而扩展卡尔曼滤波则克服了卡尔曼滤波无法处理非线性模型的问题，无迹卡尔曼滤波则使用无迹变换将非线性系统转化为线性系统进行处理，但会使滤波器系统结构参数变得更为复杂，在噪声影响、传感器失效等情况下难以确定最优滤波参数。

从控制角度讲，轮式机器人运动控制方法主要包括经典PID算法、最优控制方法、模型预测控制方法等，其中，PID算法与最优控制方法具有简单实用等优点，但参数往往难以整定或抗扰动性差。模型预测控制方法能够考虑未来预测信息并处理各种约束条件，但其在实际系统部署过程中会受到稳定性和求解效率的制约。

轮式机器人的转向主要是通过控制其主动前轮来实现的，轮式机器人主动前轮转向控制的前提是准确获取机器人的状态参数信息，由于技术限制和高昂的传感器价格，部分状态信息难以直接获取。目前大多控制研究直接假设轮式机器人状态信息可以直接获取，缺乏有效的状态估计过程，因此，需要用现有低成本的传感器来准确的估计这些难以直接获取的状态参数（如横摆角速度、质心侧偏角），并进一步结合状态信息估计与主动前轮转向开发自适应调整的机器人运动控制方法。

发明内容

本发明的目的在于提供一种基于状态估计信息输入的轮式机器人转向控制方法，以采用低成本传感器实现轮式机器人的主动前轮转向控制，利用实时准确的状态输入显著改善机器人横向稳定性。

本申请实施例提供了一种基于状态估计信息输入的轮式机器人转向控制方法，包括以下步骤：

步骤1：根据轮式机器人的运动特性，构建包含横摆角速度和质心侧偏角的理想二自由度模型，并根据理想二自由度模型构建状态空间方程，再根据状态空间方程的稳态条件计算得到理想横摆角速度；

步骤2：基于步骤1中构建的状态空间方程，建立并改进非线性系统的平方根无迹卡尔曼滤波器的离散状态方程和观测方程，并采用平方根滤波的思想对无迹卡尔曼滤波器进行横摆角速度待估计参数和质心侧偏角待估计参数的修正及递推更新，使用鲸鱼优化算法对平方根无迹卡尔曼滤波器进行非高斯噪声寻优，并获得轮式机器人的横摆角速度估计值和质心侧偏角估计值；

步骤3：构建自适应超螺旋滑模控制器，并将基于步骤2中获得的轮式机器人横摆角速度估计值与步骤1中计算的理想横摆角速度值进行作差，将二者的偏差作为构建的自适应超螺旋滑模控制器的输入，计算输出轮式机器人的主动前轮转角；

步骤4：将步骤3中自适应超螺旋滑模控制器获得的主动前轮转角输入至轮式机器人中，进行轮式机器人的主动前轮转向控制，提高轮式机器人的横向稳定性，最后返回步骤1中进行循环。

作为本申请的一些实施例，步骤1中，根据轮式机器人的运动特性，构建的理想二自由度模型如下：

式中，分别为轮式机器人的横摆角速度、质心侧偏角；上标表示对所示量的微分，分别表示的微分形式；表示前轮转角；分别表示机器人的纵向速度、侧向加速度；分别为机器人前后轴的等效侧偏刚度；分别为前轴到质心的距离、后轴到质心的距离；为质心转动惯量；为轮式机器人质量。

根据理想二自由度模型构建的状态空间方程如下：

令，可计算出理想横摆角速度为：

其中，上标表示对所示量进行转置操作，为理想横摆角速度；为机器人稳定性系数。

作为本申请的一些实施例，步骤2中，基于步骤1中构建的状态空间方程，构建并改进无迹卡尔曼滤波器，具体包括：

（2-1）建立改进无迹卡尔曼滤波器的离散状态方程和观测方程，如下式所示：

式中：为k时刻的状态变量；为k时刻的测量变量；为状态传递函数；为测量传递函数；为k-1时刻的状态变量；为输入变量；分别为过程噪声和测量噪声；基于理想二自由度模型，平方根无迹卡尔曼滤波器的状态向量设置为；测量变量设置为。

（2-2）平方根无迹卡尔曼滤波器算法步骤如下：

（2-2-1）初始化，在无迹卡尔曼滤波的基础上，通过Cholesky分解后的矩阵平方根计算状态协方差：

式中，表示期望矩阵；为状态初始值；为状态初始估计值；为状态平方根因子的下三角矩阵；chol{}表示Cholesky分解。

（2-2-2）sigma点的选择计算和时间更新，主要步骤如下：

Step1 非线性状态方程的预测和状态协方差矩阵平方根的计算：

式中，为k-1时刻sigma采样点集；为k-1时刻状态估计值；为大于0的自然数；为调节参数；为k-1时刻的状态平方根因子；为根据k-1时刻预测的k时刻的sigma采样点集；为k-1时刻的输入量；为均值的权重；为对应i时刻根据k-1时刻预测的k时刻的sigma采样点集；为k时刻的状态平方根因子；qr{}为QR分解函数；为1时刻方差的权重；为第1到2n的k时刻sigma采样点集；为k时刻状态估计值的下三角矩阵；为过程噪声协方差矩阵；为k时刻状态平方根因子的下三角矩阵；cholupdate{}为乔里斯基分解的更新函数；为初始k时刻的sigma采样点集；为初始方差的权重。

Step2 Sigma点的更新传播和非线性观测方程的进一步预测：

式中，为根据k-1时刻预测的k时刻的观测更新的Sigma点集；为估计的测量变量的下三角矩阵；为对应i时刻根据k-1时刻预测的k时刻的观测更新的Sigma点集。

相关权重因子计算如下：

式中，为初始均值的权重；为方差的权重；表示Sigma点偏离期望值的程度；表示状态的分布情况，为一种调节参数。

（2-2-3）测量更新，测量平方根因子计算为：

式中，为测量平方根因子；为第1至2n的k时刻的观测更新的Sigma点集；为初始k时刻的观测更新的Sigma点集；为测量平方根因子的下三角矩阵；为测量噪声协方差矩阵。

（2-3）卡尔曼滤波增益计算为：

式中，为互协方差矩阵；为卡尔曼滤波增益，表示对进行转置操作。

（2-4）最终得到待估计状态为：

式中，为k时刻的状态估计值，得到横摆角速度与质心侧偏角估计值为；为过程变量；为状态协方差平方根因子的下三角矩阵；为根据k-1时刻预测的k时刻的状态协方差平方根因子。

（2-5）无迹卡尔曼滤波器对应的过程噪声协方差矩阵和测量噪声协方差矩阵是鲸鱼优化算法待优化的参数，二者作为优化参数，自适应得到最优参数。

矩阵和矩阵上的元素被设置为算法寻优维度，如以下公式中所示：

式中，分别表示质心侧偏角与横摆角速度的状态噪声值；表示侧向加速度的过程噪声值；diag()表示对角矩阵。

（2-6）适应度函数为：

式中，为适应度函数；加入鲸鱼优化算法后，无迹卡尔曼滤波器可对过程噪声和测量噪声进行优化。

作为本申请的一些实施例，步骤3中，自适应超螺旋滑模控制器（自适应超螺旋滑模主动前轮转向控制器）具体包括：

根据理想二自由度动力学模型建立相对阶数为1的系统：

式中，为状态变量，为控制输入，为不确定项，为控制量参数。

首先以估计的横摆角速度估计值与理想横摆角速度的偏差作为系统的跟踪误差，表达式为：

定义滑模面为：

并根据机器人理想二自由度模型，结合步骤2中质心侧偏角估计值，可得：

根据超螺旋滑模控制算法原理，设计控制律为：

式中，为滑模控制项；为滑模切换项；为待设计参数；为符号函数；为切换器切换项的切换速度；为对的微分。

考虑到求解难度，将设定为：

式中，均为待定系数。

等效滑模控制采用滑模面导数点为最优输入解，设计的等效滑模控制输入为：

最终可得控制律并求解出主动前轮转角为：

式中，为主动前轮转角。

本申请实施例中提供的一个或多个技术方案，至少具有如下技术效果或优点：

1、本申请考虑轮式机器人的运动，建立改进滤波器对轮式机器人主动前轮转向关键状态参数进行实时估计，引入平方根滤波的思想对滤波器进行约束，采用优化算法对平方根无迹卡尔曼滤波器的非高斯噪声自适应寻优，可以在一定程度上减弱滤波过程不稳定的影响，并使得估计精度有所提高。

2、本申请提出一种自适应超螺旋滑模控制方法实现轮式机器人的主动前轮转向控制过程，采用滤波器估计的实时参数与理想参数作差计算横摆角速度输入，利用滑模控制算法设计了自适应超螺旋滑模控制器，计算附加转向角进行主动前轮转向控制，消除了抖震影响，具有较广的适用性和良好的鲁棒性。

3、本申请以轮式机器人实时反馈的行驶参数(前轮转角和侧向加速度)为实际输入量，利用系统输入参数、轮式机器人结构参数、实时测量参数，结合状态信息估计与主动前轮转向自适应调整轮式机器人控制状态，能够充分利用轮式机器人的机动性，提高了路径跟踪过程的行驶稳定性。

附图说明

图1为本申请的实施例提供的基于状态估计信息输入的轮式机器人主动前轮转向控制方法的流程图；

图2为本申请的实施例提供的基于状态估计信息输入的轮式机器人主动前轮转向控制方法的技术路线图；

图3为本申请的轮式机器人二自由度动力学模型图；

图4为本申请所提出的状态估计方法与传统无迹卡尔曼算法、真实值的对比图；

图5为本发明自适应超螺旋滑模控制器输出前轮转角导入轮式机器人的质心侧偏角对比图。

具体实施方式

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

请参阅图1、图2，本申请的实施例提供了一种基于状态估计信息输入的轮式机器人转向控制方法，并将其运用到一轮式机器人中，从而获得该转向控制方法应用后的横摆角速度估计和质心侧偏角控制曲线，该基于状态估计信息输入的轮式机器人转向控制方法包括如下步骤：

步骤1：如图3，根据轮式机器人的运动特性，构建包含横摆角速度和质心侧偏角的,理想二自由度模型，如下式所示：

式中，分别为轮式机器人的横摆角速度、质心侧偏角；上标表示对所示量的微分，分别表示的微分形式；表示前轮转角；分别表示机器人的纵向速度、侧向加速度；分别为机器人前后轴的等效侧偏刚度；分别为前轴到质心的距离、后轴到质心的距离；为质心转动惯量；为轮式机器人质量。

并根据理想二自由度模型构建状态空间方程，构建的状态空间方程如下：

再根据状态空间方程的稳态条件计算得到理想横摆角速度：即，令，计算出理想横摆角速度为：

其中，上标表示对所示量进行转置操作，为理想横摆角速度；为机器人稳定性系数。

步骤2：基于步骤1中构建的状态空间方程，构建并改进无迹卡尔曼滤波器，具体包括：

（2-1）建立改进无迹卡尔曼滤波器的离散状态方程和观测方程，如下式所示：

式中：为k时刻的状态变量；为k时刻的测量变量；为状态传递函数；为测量传递函数；为k-1时刻的状态变量；为输入变量；分别为过程噪声和测量噪声；基于理想二自由度模型，平方根无迹卡尔曼滤波器的状态向量设置为；测量变量设置为。

（2-2）采用平方根滤波的思想对无迹卡尔曼滤波器进行横摆角速度待估计参数和质心侧偏角待估计参数的修正及递推更新，平方根无迹卡尔曼滤波器算法步骤如下：

（2-2-1）初始化，在无迹卡尔曼滤波的基础上，通过Cholesky分解后的矩阵平方根计算状态协方差：

式中，表示期望矩阵；为状态初始值；为状态初始估计值；为状态平方根因子的下三角矩阵；chol{}表示Cholesky分解。

（2-2-2）sigma点的选择计算和时间更新，主要步骤如下：

Step1 非线性状态方程的预测和状态协方差矩阵平方根的计算：

式中，为k-1时刻sigma采样点集；为k-1时刻状态估计值；为大于0的自然数；为调节参数；为k-1时刻的状态平方根因子；为根据k-1时刻预测的k时刻的sigma采样点集；为k-1时刻的输入量；为均值的权重；为对应i时刻根据k-1时刻预测的k时刻的sigma采样点集；为k时刻的状态平方根因子；qr{}为QR分解函数；为1时刻方差的权重；为第1到2n的k时刻sigma采样点集；为k时刻状态估计值的下三角矩阵；为过程噪声协方差矩阵；为k时刻状态平方根因子的下三角矩阵；cholupdate{}为乔里斯基分解的更新函数；为初始k时刻的sigma采样点集；为初始方差的权重。

Step2 Sigma点的更新传播和非线性观测方程的进一步预测：

式中，为根据k-1时刻预测的k时刻的观测更新的Sigma点集；为估计的测量变量的下三角矩阵；为对应i时刻根据k-1时刻预测的k时刻的观测更新的Sigma点集。

相关权重因子计算如下：

式中，为初始均值的权重；为方差的权重；表示Sigma点偏离期望值的程度；表示状态的分布情况，为一种调节参数。

（2-2-3）测量更新，测量平方根因子计算为：

式中，为测量平方根因子；为第1至2n的k时刻的观测更新的Sigma点集；为初始k时刻的观测更新的Sigma点集；为测量平方根因子的下三角矩阵；为测量噪声协方差矩阵。

（2-3）卡尔曼滤波增益计算为：

式中，为互协方差矩阵；为卡尔曼滤波增益，表示对进行转置操作。

（2-4）最终得到待估计状态为：

式中，为k时刻的状态估计值，得到横摆角速度与质心侧偏角估计值为；为过程变量；为状态协方差平方根因子的下三角矩阵；为根据k-1时刻预测的k时刻的状态协方差平方根因子。

（2-5）使用鲸鱼优化算法对平方根无迹卡尔曼滤波器进行非高斯噪声寻优，并获得轮式机器人的横摆角速度估计值和质心侧偏角估计值，无迹卡尔曼滤波器对应的过程噪声协方差矩阵和测量噪声协方差矩阵是鲸鱼优化算法待优化的参数，二者作为优化参数，自适应得到最优参数。

矩阵和矩阵上的元素被设置为算法寻优维度，如以下公式中所示：

式中，分别表示质心侧偏角与横摆角速度的状态噪声值；表示侧向加速度的过程噪声值；diag()表示对角矩阵。

（2-6）适应度函数为：

式中，为适应度函数；加入鲸鱼优化算法后，无迹卡尔曼滤波器可对过程噪声和测量噪声进行优化。

步骤3：构建自适应超螺旋滑模控制器，具体包括：

根据理想二自由度动力学模型建立相对阶数为1的系统：

式中，为状态变量，为控制输入，为不确定项，为控制量参数。

将基于步骤2中获得的轮式机器人横摆角速度估计值与步骤1中计算的理想横摆角速度值进行作差，将二者的偏差作为构建的自适应超螺旋滑模控制器的输入，即以估计的横摆角速度估计值与理想横摆角速度的偏差作为系统的跟踪误差，表达式为：

定义滑模面为：

并根据机器人理想二自由度模型，结合步骤2中质心侧偏角估计值，可得：

根据超螺旋滑模控制算法原理，设计控制律为：

式中，为滑模控制项；为滑模切换项；为待设计参数；为符号函数；为控制器切换项的切换速度；为对的微分。

考虑到求解难度，将设定为：

式中，均为待定系数。

等效滑模控制采用滑模面导数点为最优输入解，设计的等效滑模控制输入为：

最终可得控制律并计算输出轮式机器人的主动前轮转角：

式中，为主动前轮转角。

步骤4：将步骤3中自适应超螺旋滑模控制器获得的主动前轮转角输入至轮式机器人中，进行轮式机器人的主动前轮转向控制，提高轮式机器人的横向稳定性，最后返回步骤1中进行循环。

通过将设计的基于状态估计信息输入的轮式机器人主动前轮转向控制方法应用于双移线变换工况，行进速度设置为40km/h，结果如图4和图5所示，分别给出了提出的状态估计方法与自适应超螺旋滑模控制的横摆角速度估计与质心侧偏角控制对比图，图中同时还给出了传统算法的实施结果，从而形成对比。从图中可以看出，本申请方法设计的估计方法能够更为准确有效地估计轮式机器人横摆运动状态，精度提升约10%，从而在低成本传感器配置下为轮式机器人行进过程提供准确有效的运动状态感知信息；结合估计的状态输入数据与自适应超螺旋滑模控制计算输出机器人前轮转角，实施运动控制下的质心侧偏角明显小于传统滑模控制算法，大大提升了轮式机器人的横向稳定性，加强了轮式机器人的机动性，可以适用于更为复杂行进环境下的稳定运输作业。

尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (1)

1.一种基于状态估计信息输入的轮式机器人转向控制方法，其特征在于，包括以下步骤：

步骤1：根据轮式机器人的运动特性，构建包含横摆角速度和质心侧偏角的理想二自由度模型，并根据理想二自由度模型构建状态空间方程，再根据状态空间方程的稳态条件计算得到理想横摆角速度；

步骤2：基于步骤1中构建的状态空间方程，建立并改进非线性系统的平方根无迹卡尔曼滤波器的离散状态方程和观测方程，并采用平方根滤波的思想对无迹卡尔曼滤波器进行横摆角速度待估计参数和质心侧偏角待估计参数的修正及递推更新，使用鲸鱼优化算法对平方根无迹卡尔曼滤波器进行非高斯噪声寻优，并获得轮式机器人的横摆角速度估计值和质心侧偏角估计值；

步骤3：构建自适应超螺旋滑模控制器，并将基于步骤2中获得的轮式机器人横摆角速度估计值与步骤1中计算的理想横摆角速度值进行作差，将二者的偏差作为构建的自适应超螺旋滑模控制器的输入，计算输出轮式机器人的主动前轮转角；

步骤4：将步骤3中自适应超螺旋滑模控制器获得的主动前轮转角输入至轮式机器人中，进行轮式机器人的主动前轮转向控制，提高轮式机器人的横向稳定性，最后返回步骤1中进行循环；

步骤1中，根据轮式机器人的运动特性，构建的理想二自由度模型如下：

，

式中，分别为轮式机器人的横摆角速度、质心侧偏角；上标表示对所示量的微分，分别表示的微分形式；表示前轮转角；分别表示机器人的纵向速度、侧向加速度；分别为机器人前后轴的等效侧偏刚度；分别为前轴到质心的距离、后轴到质心的距离；为质心转动惯量；为轮式机器人质量；

根据理想二自由度模型构建的状态空间方程如下：

，

令，可计算出理想横摆角速度为：

，

其中，上标表示对所示量进行转置操作，为理想横摆角速度；为机器人稳定性系数；

步骤2中，基于步骤1中构建的状态空间方程，构建并改进无迹卡尔曼滤波器，具体包括：

（2-1）建立改进无迹卡尔曼滤波器的离散状态方程和观测方程，如下式所示：

，

式中：为k时刻的状态变量；为k时刻的测量变量；为状态传递函数；为测量传递函数；为k-1时刻的状态变量；为输入变量；分别为过程噪声和测量噪声；基于理想二自由度模型，平方根无迹卡尔曼滤波器的状态向量设置为；测量变量设置为；

（2-2）平方根无迹卡尔曼滤波器算法步骤如下：

（2-2-1）初始化：在无迹卡尔曼滤波的基础上，通过Cholesky分解后的矩阵平方根计算状态协方差：

，

式中，表示期望矩阵；为状态初始值；为状态初始估计值；为状态平方根因子的下三角矩阵；chol{}表示Cholesky分解；

（2-2-2）sigma点的选择计算和时间更新，主要步骤如下：

Step1 非线性状态方程的预测和状态协方差矩阵平方根的计算：

，

式中，为k-1时刻sigma采样点集；为k-1时刻状态估计值；为大于0的自然数；为调节参数；为k-1时刻的状态平方根因子；为根据k-1时刻预测的k时刻的sigma采样点集；为k-1时刻的输入量；为均值的权重；为对应i时刻根据k-1时刻预测的k时刻的sigma采样点集；为k时刻的状态平方根因子；qr{}为QR分解函数；为1时刻方差的权重；为第1到2n的k时刻sigma采样点集；为k时刻状态估计值的下三角矩阵；为过程噪声协方差矩阵；为k时刻状态平方根因子的下三角矩阵；cholupdate{}为乔里斯基分解的更新函数；为初始k时刻的sigma采样点集；为初始方差的权重；

Step2 Sigma点的更新传播和非线性观测方程的进一步预测：

，

式中，为根据k-1时刻预测的k时刻的观测更新的Sigma点集；为估计的测量变量的下三角矩阵；为对应i时刻根据k-1时刻预测的k时刻的观测更新的Sigma点集；

相关权重因子计算如下：

，

式中，为初始均值的权重；为方差的权重；表示Sigma点偏离期望值的程度；表示状态的分布情况，为一种调节参数；

（2-2-3）测量更新，测量平方根因子计算为：

，

式中，为测量平方根因子；为第1至2n的k时刻的观测更新的Sigma点集；为初始k时刻的观测更新的Sigma点集；为测量平方根因子的下三角矩阵；为测量噪声协方差矩阵；

（2-3）卡尔曼滤波增益计算为：

，

式中，为互协方差矩阵；为卡尔曼滤波增益，表示对进行转置操作；

（2-4）最终得到待估计状态为：

，

式中，为k时刻的状态估计值，得到横摆角速度与质心侧偏角估计值为；为过程变量；为状态协方差平方根因子的下三角矩阵；为根据k-1时刻预测的k时刻的状态协方差平方根因子；

（2-5）无迹卡尔曼滤波器对应的过程噪声协方差矩阵和测量噪声协方差矩阵是鲸鱼优化算法待优化的参数，二者作为优化参数，自适应得到最优参数；

矩阵和矩阵上的元素被设置为算法寻优维度，如以下公式中所示：

，

，

，

式中，分别表示质心侧偏角与横摆角速度的状态噪声值；表示侧向加速度的过程噪声值；diag()表示对角矩阵；

（2-6）适应度函数为：

，

式中，为适应度函数；加入鲸鱼优化算法后，无迹卡尔曼滤波器可对过程噪声和测量噪声进行优化；

步骤3中，自适应超螺旋滑模控制器具体包括：

根据理想二自由度动力学模型建立相对阶数为1的系统：

，

式中，为状态变量，为控制输入，为不确定项，为控制量参数；

首先以估计的横摆角速度估计值与理想横摆角速度的偏差作为系统的跟踪误差，表达式为：

，

定义滑模面为：

，

并根据机器人理想二自由度模型，结合步骤2中质心侧偏角估计值，可得：

，

根据超螺旋滑模控制算法原理，设计控制律为：

，

式中，为滑模控制项；为滑模切换项；为待设计参数；为符号函数；为控制器切换项的切换速度；为对的微分；

将设为：

，

式中，均为待定系数；

等效滑模控制采用滑模面导数点为最优输入解，设计的等效滑模控制输入为：

，

最终可得控制律并求解出主动前轮转角为：

，

式中，为主动前轮转角。