一种履带式移动机器人的自主行驶路径跟踪控制方法

摘要

本发明提供一种履带式移动机器人的自主行驶路径跟踪控制方法，能够提高移动机器人路径跟踪时的准确性和可靠性。所述方法包括：获得履带式移动机器人当前的位姿信息、质心速度信息及驱动轮转速信息；根据获得的信息，通过履带式移动机器人的运动学与动力学模型，确定预测时域内履带式移动机器人的位姿信息，并将确定的位姿信息与期望的路径信息进行比对，得到预测时域内的位姿偏差；以最小化路径跟踪过程中的预测位姿偏差、预测控制量增量、预测航向角速度变化之和为目标，确定非线性模型预测控制的优化函数，并通过结合约束条件最小化优化函数得到最优控制序列，以便所述履带式移动机器人根据最优控制序列输出位姿信息跟踪期望路径自主行驶。

说明书

技术领域

本发明涉及移动机器人自主行驶控制领域，特别是指一种履带式移动机器人的自主行驶路径跟踪控制方法。

背景技术

近年来，履带式移动机器人被广泛应用于军事、搜救和勘探等领域。自主行驶移动机器人系统通常包括地图绘制，定位，感知和导航，其中路径跟踪是导航系统的核心部分。路径跟踪控制的目的是让移动机器人尽可能准确地按照期望路径自主行驶。对于移动机器人的路径跟踪控制，目前已有的包括模糊控制(Fuzzy Control，FC)策略、预瞄比例积分微分(Proportion Integration Differentiation，PID)控制、滑模控制(Sliding ModeControl，SMC)、线性二次型调节器(Linear Quadratic Regulator，LQR)控制和神经网络(Neural Networks)控制等。

现有的针对履带式移动机器人的路径跟踪控制方法，一般都是基于运动学模型，但是运动学模型仅考虑履带式移动机器人的几何关系，没有考虑履带式移动机器人动力、环境阻力及履带式移动机器人在行驶过程中产生的侧滑、滑移等情况，因此在非结构化道路、高速以及急弯等的情况下，单独使用运动学模型描述履带式移动机器人的路径跟踪情况，将产生很大的跟踪误差。

发明内容

本发明要解决的技术问题是提供一种履带式移动机器人的自主行驶路径跟踪控制方法，以解决现有技术所存在的单独使用运动学模型描述履带式移动机器人的路径跟踪情况，导致跟踪误差大的问题。

为解决上述技术问题，本发明实施例提供一种履带式移动机器人的自主行驶路径跟踪控制方法，包括：

获取履带式移动机器人当前的位姿信息、质心速度信息及驱动轮转速信息；

根据获得的位姿信息、质心速度信息及驱动轮转速信息，通过履带式移动机器人的运动学与动力学模型，确定预测时域内履带式移动机器人的位姿信息，并将确定的位姿信息与期望的路径信息进行比对，得到预测时域内的位姿偏差；

以最小化路径跟踪过程中的预测位姿偏差、预测控制量增量、预测航向角速度变化之和为目标，确定非线性模型预测控制的优化函数，并通过结合约束条件最小化优化函数得到最优控制序列，以便所述履带式移动机器人根据最优控制序列输出位姿信息跟踪期望路径自主行驶。

进一步地，所述履带式移动机器人的运动学模型为：

其中，表示履带式移动机器人质心c点在全局坐标系(X_G,Y_G)下X_G方向速度分量；表示履带式移动机器人质心c点在全局坐标系下的速度标量值；α表示滑动转向造成的滑移角；r表示履带式移动机器人驱动轮半径；ω_l表示履带式移动机器人左侧驱动轮转速；ω_r表示履带式移动机器人右侧驱动轮转速；σ_l表示左侧履带滑转率；σ_r表示右侧履带滑转率；表示履带式移动机器人质心c点在全局坐标系(X_G,Y_G)下Y_G方向速度分量；ω均表示航向角速度；R表示履带式移动机器人转向半径；b表示两侧履带中心距。

进一步地，滑移角α表示为：

进一步地，履带式移动机器人转向半径R表示为：

其中，v_l表示履带式移动机器人左驱动轮转速；v_r表示履带式移动机器人右驱动轮转速。

进一步地，所述履带式移动机器人的动力学模型为：

其中，表示履带式移动机器人质心c点在全局坐标系(X_G,Y_G)下X_G方向加速度分量；F_l、F_r分别表示左侧履带、右侧履带纵向驱动力；R_l、R_r分别表示履带式移动机器人左侧、右侧纵向阻力；m表示履带式移动机器人质量；表示履带式移动机器人质心c点在全局坐标系(X_G,Y_G)下Y_G方向加速度分量；sgn(·)表示符号函数；μ_lat表示横向阻力系数；u表示瞬心偏移距离；l表示履带接地长度；G表示履带式移动机器人的重量；表示履带式移动机器人航向角加速度；I表示履带式移动机器人转动惯量。

进一步地，左侧履带、右侧履带纵向驱动力F_l、F_r分别表示为：

其中，F_lmax、F_rmax分别为左侧履带、右侧履带在特定的土壤环境下所能产生的最大的切线牵引力，h表示单条履带宽度，c’表示土壤内聚力，表示土壤内摩擦角；K表示土壤的水平剪切变形模量。

进一步地，履带式移动机器人左侧、右侧纵向阻力R_l、R_r表示为：

其中，n表示土壤变形指数；b’表示履带宽度；p_a表示履带平均接地比压；Z_a表示履带平均沉陷深度；K_c表示土壤粘性成分所决定的变形模量；表示土壤摩擦性成分所决定的变形模量。

进一步地，所述位姿信息包括：位置及航向角；所述质心速度信息包括：速度大小和速度方向；所述驱动轮转速信息包括：左右两驱动轮角速度大小和方向；

所述根据获得的位姿信息、质心速度信息及驱动轮转速信息，通过履带式移动机器人的运动学与动力学模型，确定预测时域内履带式移动机器人的位姿信息，并将确定的位姿信息与期望的路径信息进行比对，得到预测时域内的位姿偏差包括：

确定运动学、动力学模型的状态量x和控制量u，其中，状态量x为履带式移动机器人质心c在全局坐标系(X_G,Y_G)中的位置坐标(X、Y)、速度分量和履带式移动机器人航向角θ、航向角速度以及由于滑动转向造成的滑移角α，即控制量u为履带式移动机器人左右两侧的驱动轮转速，即u＝[ω_lω_r]；

确定状态量x与控制量u之间的关系为：

将状态量x与控制量u之间的关系描述为其中，表示状态量x的微分，表示x、u与的函数关系；

对所述履带式移动机器人的运动学、动力学模型进行离散化，建立预测模型，根据获得的履带式移动机器人在全局坐标系(X_G,Y_G)中当前的位姿信息、质心速度信息及驱动轮转速信息，通过预测模型预测未来一段时间[t,t+N_p]内的位姿信息，所述预测模型表示为：

其中，T表示采样间隔，x(i|t)为t时刻第i个预测点的状态量，u(i|t)为t时刻第i个预测点的控制量，N_p为预测时域，N_c表示控制时域；

将预测得到的位姿信息与期望的路径信息进行比对，得到预测时域内的位姿偏差，所述位姿偏差表示为：

其中，e(i|t)为t时刻第i个预测点与期望的参考点之间的位姿偏差；x′(i|t)为t时刻第i个预测点的位姿信息，x′(i|t)＝[X(i|t)Y(i|t)θ(i|t)]^T，X(i|t)、Y(i|t)、θ(i|t)分别为t时刻第i个预测点的X坐标、Y坐标和航向角；x_ref(i|t)为t时刻第i个参考点的位姿信息，x_ref(i|t)＝[X_ref(i|t)Y_ref(i|t)θ_ref(i|t)]^T，X_ref(i|t)、Y_ref(i|t)、θ_ref(i|t)分别为t时刻第i个参考点的X坐标、Y坐标和航向角。

进一步地，所述非线性模型预测控制的优化函数表示为：

其中，J表示优化函数，Δu(i|t)表示控制量增量，u(i|t)＝[ω_l(i|t),ω_r(i|t)]^T，ω_l(i|t),ω_r(i|t)分别为履带式移动机器人左右两侧驱动轮在控制时域内t时刻第i个预测点的转速，P、Q、R都表示权重矩阵；所述优化函数J的约束条件为：

其中，Δω_l,Δω_r分别为履带式移动机器人左右两侧驱动轮在采样间隔T时间段内的增量变化，ω_*，min、ω_*，max分别为相应侧的驱动轮转速的最小值和最大值，Δω_*，min，Δω_*，max分别为相应的驱动轮在采样间隔T时间段内的增量变化的最小值和最大值，*的取值为左侧l、右侧r。

进一步地，最优控制序列其中，为t时刻第1个最优控制量，将该最优控制序列中第一个元素作为履带式移动机器人的实际控制量控制所述履带式移动机器人。

本发明的上述技术方案的有益效果如下：

上述方案中，获取履带式移动机器人当前的位姿信息、质心速度信息及驱动轮转速信息；根据获得的位姿信息、质心速度信息及驱动轮转速信息，通过履带式移动机器人的运动学与动力学模型，确定预测时域内履带式移动机器人的位姿信息，并将确定的位姿信息与期望的路径信息进行比对，得到预测时域内的位姿偏差；以最小化路径跟踪过程中的预测位姿偏差、预测控制量增量、预测航向角速度变化之和为目标，确定非线性模型预测控制的优化函数，并通过结合约束条件最小化优化函数得到最优控制序列，以便所述履带式移动机器人根据最优控制序列输出位姿信息跟踪期望路径自主行驶。这样，通过基于运动学模型和动力学模型的非线性模型预测控制方法，能够使履带式移动机器人跟踪期望路径自主行驶，从而提高履带式移动机器人在非结构化道路、高速以及急弯等的情况下进行路径跟踪时的准确性和可靠性。

附图说明

图1为本发明实施例提供的履带式移动机器人的自主行驶路径跟踪控制方法的传感器安装示意图；

图2为本发明实施例提供的履带式移动机器人的自主行驶路径跟踪控制方法的流程示意图；

图3为本发明实施例提供的履带式移动机器人的运动学模型示意图；

图4为本发明实施例提供的履带式移动机器人的动力学模型示意图；

图5为本发明实施例提供的履带式移动机器人的自主行驶路径跟踪控制原理示意图；

图6为本发明实施例提供的履带式移动机器人的自主行驶路径跟踪控制方法的详细流程示意图。

具体实施方式

为使本发明要解决的技术问题、技术方案和优点更加清楚，下面将结合附图及具体实施例进行详细描述。

本发明针对现有的单独使用运动学模型描述履带式移动机器人的路径跟踪情况，导致跟踪误差大的问题，提供一种履带式移动机器人的自主行驶路径跟踪控制方法。

传感器系统是移动机器人(例如，履带式移动机器人)进行自主行驶的硬件基础，在移动机器人自主行驶过程中，传感器系统可以为移动机器人的控制系统提供必要的输入。

如图1所示，本实施例中，需要使用的传感器包括但不限于：激光雷达系统(包括：2个激光雷达1)、超声波测距仪3、惯性导航系统5、编码器4及摄像头2；为了更好地理解更传感器的作用，对上述传感器进行简要说明：

1)激光雷达系统

如图1所示，所述激光雷达系统包括：2个激光雷达，用于提供障碍物信息；其中一个激光雷达安装在所述移动机器人前梁上部，该激光雷达用于测量机器人前部方向障碍物的位置信息；另一个激光雷达安装在移动机器人后梁上部，该激光雷达用于检测机器人后方障碍物的位置信息。

本实施例中，所述激光雷达系统与工控机使用网口通过TCP/IP进行连接，由工控机向激光雷达发送请求，然后激光雷达通过网络接口设定频率发送数据包。对于网口的传输方式，进行数据处理前，首先需要把ASCII码表达的数值转化为对应的二进制数值。激光雷达为16线激光雷达，此激光雷达可以获取水平360°和垂直方向30°以内移动机器人周围环境障碍物的三维信息。当移动机器人前方或者后方有障碍物时就可以通过激光雷达获取移动机器人前后方的道路状况信息，这些信息包括障碍物的长、宽、高及属性分类。

本实施例中，为了保证激光雷达系统的正常工作，需要对激光雷达安装参数进行标定，激光雷达系统与移动机器人为刚性连接，为了方便激光雷达坐标系与移动机器人坐标系的转换，需要对激光雷达的安装进行简单标定，并使激光雷达数据从激光雷达坐标系统转换到移动机器人坐标系上。激光雷达外部安装参数的标定需要使用等腰直角三角形标定板和正方形标定板来完成，需要标定的安装参数包括俯仰角和侧倾角。

本实施例中，对于所述激光雷达系统，需要进行数据转换。由于移动机器人前后各安装有一个激光雷达，前后两个雷达由于各自的安装位置和外部参数的不同，所得到的环境信息表示方法也不大相同。所以需要通过建立统一的坐标系，将两个激光雷达的数据转换到统一的基准坐标系下。

2)超声波测距仪

如图1所示，超声波测距仪安装在移动机器人底部，具有超声波发射探头和超声波接收探头，超声波发射探头用于发射超声波，超声波接收探头用于接收由障碍物反射回来的超声波，根据公式确定移动机器人底部到地面的垂直距离，其中，h_t为移动机器人底盘到地面的垂直距离，v为超声波传播的速度，t为超声波接收探头接收到发射信号所需要的时间。

本实施例中，所述超声波测距仪的作用是通过检测移动机器人底部与地面的垂直距离，计算出履带的平均压陷深度，为使用非线性模型预测控制(Nonlinear ModelPredictive Control,NMPC)提供必要的控制器输入量。

3)惯性导航系统

如图1所示，所述惯性导航系统包括：3个陀螺仪和3个加速度计等敏感元件，分别用来感应移动机器人三个坐标轴方向上的角速度和线加速度。惯性导航系统根据陀螺仪的输出建立导航坐标系，根据加速度计输出移动机器人在导航坐标系中的速度和位置。

本实施例中，所述惯性导航系统的自身误差或者随机误差引起的测量值偏差会在积分运算过程中随时间无限增长。因此需要采用激光雷达进行辅助惯性导航，准确估计并补偿惯性导航系统误差，修正惯性导航解算。激光雷达观测量为两次扫描之间匹配直线特征参数变化。惯性导航系统预测的两个扫描之间匹配直线特征参数变化用于辅助直线特征匹配和与激光雷达的观测值进行比较，参数变化的差值传递到扩展卡尔曼滤波器中进行误差状态估计。

4)编码器

如图1所示，两个编码器分别安装于所述履带式移动机器人两侧履带的驱动轮轴上，用于测量两侧驱动轮转速，用以获取履带的实际转速。编码器通过CAN总线将获取的转速数据传入工控机。

5)摄像头

如图1所示，摄像头安装于移动机器人顶部，用于提供路面信息。建立摄像机环境坐标系、摄像机局部坐标系和图像坐标系之间的对应关系。通过将三维空间中的点透视变换到图像平面上，将三维空间上的物体从环境坐标系转换至图像坐标系。摄像机环境坐标系中的点P(x_vc,y_vc,z_vc)到摄像机坐标系O_cx_cy_cz_c下的点p(x_c,y_c,z_c)的转换方程为：

其中，为坐标转换矩阵，表示摄像机相对于环境坐标系的安装角度，为坐标平移矩阵，表示摄像机相对于环境坐标系的安装位置。

本实施例中，坐标转换矩阵表示为：

其中，为摄像头安装位置的航向角，δ为摄像头安装位置的俯仰角，ξ为摄像头安装位置的侧倾角。

如图2所示，本发明实施例提供的履带式移动机器人的自主行驶路径跟踪控制方法，包括：

S101，获取履带式移动机器人当前的位姿信息、质心速度信息及驱动轮转速信息；

S102，根据获得的位姿信息、质心速度信息及驱动轮转速信息，通过履带式移动机器人的运动学与动力学模型，确定预测时域内履带式移动机器人的位姿信息，并将确定的位姿信息与期望的路径信息进行比对，得到预测时域内的位姿偏差；

S103，以最小化路径跟踪过程中的预测位姿偏差、预测控制量增量、预测航向角速度变化之和为目标，确定非线性模型预测控制的优化函数，并通过结合约束条件最小化优化函数得到最优控制序列，以便所述履带式移动机器人根据最优控制序列输出位姿信息跟踪期望路径自主行驶。

本发明实施例所述的履带式移动机器人的自主行驶路径跟踪控制方法，获取履带式移动机器人当前的位姿信息、质心速度信息及驱动轮转速信息；根据获得的位姿信息、质心速度信息及驱动轮转速信息，通过履带式移动机器人的运动学与动力学模型，确定预测时域内履带式移动机器人的位姿信息，并将确定的位姿信息与期望的路径信息进行比对，得到预测时域内的位姿偏差；以最小化路径跟踪过程中的预测位姿偏差、预测控制量增量、预测航向角速度变化之和为目标，确定非线性模型预测控制的优化函数，并通过结合约束条件最小化优化函数得到最优控制序列，以便所述履带式移动机器人根据最优控制序列输出位姿信息跟踪期望路径自主行驶。这样，通过基于运动学模型和动力学模型的非线性模型预测控制方法，能够使履带式移动机器人跟踪期望路径自主行驶，从而提高履带式移动机器人在非结构化道路、高速以及急弯等的情况下进行路径跟踪时的准确性和可靠性。

本实施例中，所述非线性模型预测控制是基于运动学模型和动力学模型确定的，在设计基于NMPC的履带式移动机器人的自主行驶路径跟踪控制器时，两个关键的步骤是建立非线性模型预测控制的预测模型和设计滚动优化功能；其中，所述预测模型基于履带式移动机器人当前姿态和当前控制输入来预测未来履带式移动机器人姿态。

为了更好地理解本发明实施例所述的履带式移动机器人的自主行驶路径跟踪控制方法，对其进行详细说明，具体可以包括以下步骤：

S101，获取履带式移动机器人当前的位姿信息、质心速度信息及驱动轮转速信息。

本实施例中，所述位姿信息包括：位置及航向角；所述质心速度信息包括：速度大小和速度方向；所述驱动轮转速信息包括：左右两驱动轮角速度大小和方向。

本实施例中，所述位置为履带式移动机器人质心c在全局坐标系(X_G,Y_G)中的坐标值。

本实施例中，可以通过惯性导航系统获取履带式移动机器人当前的位置、航向角及质心速度信息。

本实施例中，所述速度大小和速度方向为履带式移动机器人质心c在全局坐标系中的速度大小和方向。

本实施例中，可以通过编码器获取履带式移动机器人驱动轮转速大小和方向。

S102，根据获得的位姿信息、质心速度信息及驱动轮转速信息，通过履带式移动机器人的运动学与动力学模型，确定预测时域内履带式移动机器人的位姿信息，并将确定的位姿信息与期望的路径信息进行比对，得到预测时域内的位姿偏差。

本实施例中，为了在全局坐标系(X_G,Y_G)中，建立履带式移动机器人的运动学与动力学模型，做了以下假设：

1)假设履带式移动机器人的质心位于移动机器人的几何中心处；

2)假设履带式移动机器人在较为平坦路面上行驶；

3)假设履带式移动机器人在行驶过程中，路面土壤性质不改变；

3)假设履带式移动机器人在转向过程中速度较慢，离心力忽略不计；

5)忽略横向、纵向空气动力学。

本实施例中，为了执行S102，需先建立履带式移动机器人的运动学模型，如图3所示，履带式移动机器人的质心c的速度在全局坐标系(X_G,Y_G)中可以表示为：

其中，表示履带式移动机器人质心c点在全局坐标系(X_G,Y_G)下的速度矢量，表示履带式移动机器人质心c点在全局坐标系(X_G,Y_G)下X_G方向速度分量，表示履带式移动机器人质心c点在全局坐标系(X_G,Y_G)下Y_G方向速度分量，表示履带式移动机器人质心c点在全局坐标系(X_G,Y_G)下的速度标量值；r表示履带式移动机器人驱动轮半径，α表示滑动转向造成的滑移角，ω_l表示履带式移动机器人左侧驱动轮转速，rad/s；ω_r表示履带式移动机器人右侧驱动轮转速，rad/s；σ_l表示左侧履带滑转率；σ_r表示右侧履带滑转率。

本实施例中，履带式移动机器人转向半径R可以表示为：

其中，b表示两侧履带中心距，m；v_l表示机器人左驱动轮转速，m/s；v_r表示机器人右驱动轮转速，m/s。

本实施例中，履带式移动机器人航向角速度ω为：

本实施例中，履带式移动机器人的运动学模型为：

其中，表示航向角速度。

本实施例中，为了执行S102，还需建立履带式移动机器人的动力学模型，如图4所示，根据欧拉方程先建立履带式移动机器人的动力学方程：

其中，m表示履带式移动机器人质量，表示履带式移动机器人质心c点在全局坐标系(X_G,Y_G)下X_G方向加速度分量，表示履带式移动机器人质心c点在全局坐标系(X_G,Y_G)下Y_G方向加速度分量，表示履带式移动机器人航向角加速度，I表示履带式移动机器人转动惯量，F_l、F_r分别表示左侧履带、右侧履带纵向驱动力，R_l、R_r分别表示履带式移动机器人左侧、右侧纵向阻力，F_lat表示履带式移动机器人横向阻力，M表示履带式移动机器人转弯时的转向力矩，M_r表示履带式移动机器人转弯时由土壤造成的转向阻力矩。

本实施例中，左侧履带、右侧履带纵向驱动力F_l、F_r分别表示为：

其中，G表示履带式移动机器人的重量，N；F_lmax、F_rmax分别为左侧履带、右侧履带在特定的土壤环境下所能产生的最大的切线牵引力；h表示单条履带宽度，m；c’表示土壤内聚力，Pa；表示土壤内摩擦角，rad；l表示履带接地长度，m；K表示土壤的水平剪切变形模量。

本实施例中，履带式移动机器人左侧、右侧纵向阻力R_l、R_r表示为：

其中，n表示土壤变形指数；b’表示履带宽度，m；p_a表示履带平均接地比压，kPa；Z_a表示履带平均沉陷深度，m；K_c表示土壤粘性成分所决定的变形模量，kN/mⁿ⁺¹；表示土壤摩擦性成分所决定的变形模量，kN/mⁿ⁺²。

本实施例中，履带式移动机器人横向阻力F_lat可由下式计算(假设质心与几何中心重合的情况下)：

其中，sgn(·)表示符号函数；μ_lat表示横向阻力系数；u表示瞬心偏移距离，m；

本实施例中，履带式移动机器人转弯时的转向力矩M为：

本实施例中，履带式移动机器人转弯时由土壤造成的转向阻力矩M_r为：

根据上述描述，对履带式移动机器人的动力学方程：进行更新，得到履带式移动机器人的动力学模型为：

本实施例中，动力学模型考虑了履带式移动机器人动力、环境阻力等影响因素，还考虑了履带式移动机器人在行驶过程中产生的侧滑、滑移等情况，可以更加准确地描述履带式移动机器人的行驶情况。

本实施例中，所述运动学、动力学模型的状态量x为履带式移动机器人质心c在全局坐标系(X_G,Y_G)中的位置坐标(X,Y)、速度分量和履带式移动机器人航向角θ、航向角速度以及由于滑动转向造成的滑移角α，即所述运动学、动力学模型的控制量u为履带式移动机器人左右两侧的驱动轮转速，即u＝[ω_l>r]，故状态量x与控制量u的关系可以写为：

上式可以简化描述为其中，矩阵代表状态量x的微分，表示x、u与的函数关系。

在模型预测控制理论中，NMPC结合当前的传感器测量值(包括机器人位姿、驱动轮转速，机器人速度等)与被控对象(本实施例中指履带式移动机器人)的预测模型，预测被控对象未来一段时间[t,t+N_p]内的系统输出，其中时间长度N_p即称为预测时域。控制器通过对由所设优化函数和约束条件组成的优化问题进行滚动求解，可求得在未来一段时间[t,t+N_c]内的控制序列，其中，时间长度N_c称为控制时域；然后再将该控制序列的第一个控制量元素作为实际控制量输入到被控对象中。

接着，根据建立的履带式移动机器人的运动学与动力学模型，建立NMPC的预测模型，所述预测模型通过非线性运动学、动力学模型、履带式移动机器人当前的状态量和当前的控制量进行预测。

本实施例中，为了建立预测模型，首先需要对所述履带式移动机器人的运动学、动力学模型进行离散化，通过采用单步欧拉法将运动学、动力学模型改写为迭代方程，通过迭代方程可以使用当前状态量预测下一时刻状态量，且考虑到采样间隔T非常小，所以运动学、动力学模型可以离散化为：

x(i+1|t)＝x(i|t)+Tf_d(x(i|t),u(i|t))

其中，T表示采样间隔，x(i|t)为t时刻第i个预测点的状态量，u(i|t)为t时刻第i个预测点的控制量；

然后，基于离散化后的运动学、动力学模型，建立预测模型(即：状态预测方程)：

本实施例中，如图5所示，履带式移动机器人的决策与规划层可以根据激光雷达、毫米波雷达、视觉系统、惯性导航系统等检测到信息后进行路径规划，将规划好的路径信息(即：期望的路径信息)x_ref传递到NMPC中执行。

本实施例中，NMPC根据获得的履带式移动机器人在全局坐标系(X_G,Y_G)中当前的位姿信息(位置、航向角)、质心速度信息(速度大小和方向)和驱动轮转速信息(角速度大小和方向)，通过预测模型预测未来一段时间[t,t+N_p]内的位姿信息，将预测得到的位姿信息与规划的路径信息x_ref进行比对，获得预测时域内的位姿偏差(X_G方向偏差、Y_G方向偏差和航向角θ偏差)，所述位姿偏差表示为：

其中，e(i|t)为t时刻第i个预测点与期望的参考点之间的位姿偏差；x′(i|t)为t时刻第i个预测点的位姿信息，x′(i|t)＝[X(i|t)Y(i|t)θ(i|t)]^T，X(i|t)、Y(i|t)、θ(i|t)分别为t时刻第i个预测点的X坐标、Y坐标和航向角；x_ref(i|t)为t时刻第i个参考点的位姿信息，x_ref(i|t)＝[X_ref(i|t)Y_ref(i|t)θ_ref(i|t)]^T，X_ref(i|t)、Y_ref(i|t)、θ_ref(i|t)分别为t时刻第i个参考点的X坐标、Y坐标和航向角。

在得到预测时域内的位姿偏差后，需要确定NMPC的优化函数。对于履带式移动机器人的路径跟踪控制，优化目标包括减小位置误差、保证履带机器人转向平滑和保证机器人在小扰动下能够沿直线运动。

在本实施例中，使用了一个二次型优化函数计算控制序列，该二次型优化函数包括在路径跟踪过程中的预测位姿偏差之和、预测控制量增量之和与预测航向角速度之和。预测控制的目标是通过最小化优化函数J获得一段未来时间的最优控制序列为t时刻第1个最优控制量，履带式移动机器人将根据最优控制序列中输出x′(t+i|t)跟踪参考值x_ref(i+1|t)，具体可以包括以下步骤：

1)在实现履带式移动机器人路径跟踪的过程中，最重要的就是使得履带式移动机器人跟踪期望路径自主行驶，所以为了减小履带式移动机器人实际位置和期望路径之间的位姿偏差，优化函数需要惩罚在预测时域内的位姿偏差之和，位姿偏差e即为履带式移动机器人预测位姿x′与参考位姿x_ref的差值，履带式移动机器人位姿包括机器人在全局坐标系的横坐标X、纵坐标Y和航向角θ，因此设置优化函数为：

2)由于履带式移动机器人的控制量增量影响履带式移动机器人自主行驶的稳定性，单位时间内控制量变化过大会使得履带式移动机器人突然加速或者突然减速，严重影响其正常行驶，所以为了保证履带式移动机器人转向平滑且运动稳定，优化函数需要惩罚预测时域内的履带式移动机器人控制量增量之和，即惩罚履带式移动机器人的驱动轮转动速度变化之和，因此设置优化函数为：

其中，Δu(i|t)表示控制量增量，u(i|t)＝[ω_l(i|t),ω_r(i|t)]^T，ω_l(i|t),ω_r(i|t)分别为履带式移动机器人左右两侧驱动轮在控制时域内t时刻第i个预测点的转速。

3)由于履带式移动机器人存在滑动转向的特性，在进行路径跟踪时频繁的转向将严重影响履带式移动机器人的跟踪效果，履带式移动机器人的航向角速度影响履带式移动机器人的转向效果，所以为了保证履带式移动机器人能够在小扰动下仍然沿直线行驶，优化函数需要惩罚在预测时域内航向角速度变化之和，因此设置优化函数为：

综合上述三种情况，所以非线性模型预测控制的优化函数为：

其中，||·||表示范数，P、Q、R都表示权重矩阵，P、Q、R通常选取为常数；其中，P的值越大，求解优化函数后得到的控制量将使得履带式移动机器人的跟踪误差减小，但行驶平稳性和在小扰动下的直线行驶稳定性变差，同理Q的值越大，履带式移动机器人的行驶平稳性将变好，但是跟踪误差会变大，在小扰动下的直线行驶稳定性将变差，R的值越大，机器人在小扰动下直线行驶的稳定性将变好，但是跟踪误差会加大，行驶的平稳性也会变差。所以协调好P、Q、R三个矩阵的大小也相当重要，三个数的取值往往根据经验取得。

最后，还需要确定所述优化函数的约束条件。

本实施例中，由于所述履带式移动机器人两侧履带驱动力是在某个范围内的，所以两侧驱动轮的转速存在最大最小值，即存在驱动轮转速约束为ω_*，min、ω_*，max分别为相应侧的驱动轮转速的最小值和最大值。

由于履带式移动机器人的驱动轮转速存在约束条件，而控制增量是在一个采样时间内执行的，所以单位采样时间内的控制增量也存在约束条件，即Δω_l,Δω_r分别为履带式移动机器人左右两侧驱动轮在采样间隔T时间段内的增量变化，Δω_*，min，Δω_*，max分别为相应的驱动轮在采样间隔T时间段内的增量变化的最小值和最大值。

结合上式中的优化函数和约束条件，所述非线性模型预测控制的优化函数可以表示为以下形式：

s.t.

本实施例中，路径跟踪即在每一个采样时间内求解上述有约束的优化问题，通过求解该问题，若无最优解，将再次优化求解，若有最优解，则可以得到最优控制序列其中，为t时刻第1个最优控制量，将该控制序列中第一个元素作为履带式移动机器人的实际控制输入量传入履带式移动机器人平台跟踪期望路径，如图6所示。在下一控制周期，NMPC以下一时刻为初始状态再次求解上述优化问题，并随之将求解结果控制序列中第一个元素作为履带式移动机器人的实际控制量，依据NMPC的基本原理，依次循环往复执行。

被控履带式移动机器人根据NMPC给出的控制量驱动主动轮按照相应的转速转动，使得被控履带式移动机器人当前的驱动轮转速与期望驱动轮转速一致，实现履带式移动机器人的自主直线行驶与自主转向运动。

以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明所述原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。