CN109857098A - 一种基于mpc的自动驾驶农机轨迹跟踪系统及方法 - Google Patents

Abstract

本发明涉及一种基于MPC的自动驾驶农机轨迹跟踪系统及方法，包括用于采集农机周围信息的环境感知模块、用于用户定义路径并生成离散参考路径点的路径生成模块、用于将收集到的信号进行计算的决策控制模块、用于农机出现故障时实现紧急制动的紧急控制模块以及实现车辆运行及停止的执行模块；本发明可以适应农机不同作业场景，避免了高速条件下跟踪误差偏大的问题，确保了跟踪精度以及稳定性。

Description

一种基于MPC的自动驾驶农机轨迹跟踪系统及方法

技术领域

本发明涉及一种基于MPC的自动驾驶农机轨迹跟踪系统及方法，属于智能控制领域。

背景技术

农业生产是人类繁衍生存最基础的物资来源，当前我国仍是世界上最主要的农业型国家，需要用仅占世界耕地7％的土地，解决占世界人口22％的温饱问题，是一个人口较多、耕地面积较少的国家，是个农业大国，却是个农业弱国，为满足社会经济发展和人民生活水平逐渐提高的需要，就要加快发展农业机械化，推进农业精准化、精细化，发展精准农业。

同时，由于当前农业生产收益年年下滑、生产成本节节攀升，严重打击了农业从业人员的生产，农业劳动力投入逐渐不足，推进无人驾驶农机发展，提高粮食产量，改善农业智能化水平刻不容缓。

当前农机路径跟踪已经成为农机智能化的研究热点。自动驾驶包含两方面内容，一是导航定位技术，二是路径跟踪技术。导航技术主要是采用传感器进行车辆定位，依赖于传感器精度，可靠性。路径跟踪则是控制车辆按照规划好的路线行驶，跟踪精度直接关乎农机装备的可靠性，影响农机作业精准度。

发明内容

本发明提供一种基于MPC的自动驾驶农机轨迹跟踪系统及方法，可以适应农机不同作业场景，避免了高速条件下跟踪误差偏大的问题，确保了跟踪精度以及稳定性。

本发明解决其技术问题所采用的技术方案是：

一种基于MPC的自动驾驶农机轨迹跟踪系统，包括用于采集农机周围信息的环境感知模块、用于用户定义路径并生成离散参考路径点的路径生成模块、用于将收集到的信号进行计算的决策控制模块、用于农机出现故障时实现紧急制动的紧急控制模块以及实现车辆运行及停止的执行模块；

前述的环境感知模块采集农机的位置、航向角等信息，并将收集到的信息传输至决策控制模块处，路径生成模块用于用户定义路径并生成离散参考路径点后传输至决策控制模块处，决策控制模块将收集到的信号进行计算，获取当前农机的路径及期望路径偏差，计算出转角控制量，紧急制动模块用于农机在出现故障时向执行模块发出指令实现紧急制动，执行模块从上级接收转角以及制动信号，实现车辆转向及减速停止；

作为本发明的进一步优选，前述的路径生成模块包含显示屏和路径生成控制器；环境感知模块包括安装在智能农机内的组合导航系统和转角传感器，紧急制动模块包括继电器，执行模块包括步进电机和制动系统；

一种基于MPC的自动驾驶农机轨迹跟踪方法，包括以下运行步骤：

第一步：路径生成模块通过CAN总线将将生成的离散期望轨迹点传递给决策控制模块；

第二步：环境感知模块通过串口将农机的位置、航向、前轮实际转角信息传递给决策控制模块；

第三步：决策控制模块由环境感知模块处获取的信息计算当前状态量，决策控制模块由路径生成模块处获取的信息计算期望状态的误差，得出最优控制量，并将控制量的实际输出传递给执行模块；

第四步：执行模块通过驱动步进电机带动农机转向，在下一个周期内继续重复上述过程，不断减小与参考轨迹的误差，直到跟踪结束；

第五步：如遇紧急情况，紧急控制模块驱动执行模块进行紧急制动操作；

作为本发明的进一步优选，前述决策控制模块的处理过程包括以下步骤：

A：环境感知模块获取农机实时位置信息，结合农机结构参数，通过二自由度模型理论，进行动力学建模，具体参数为

其中：

其中，定义CG为农机的质心，γ为农机的横摆角速度，β为质心侧偏角，δ_f为前轮转角，C_f为前轮侧偏刚度，C_r为后轮侧偏刚度，I_z为绕通过其质心CG的垂直轴线转动惯量，前后轮到CG距离为l_f和l_r，v_x和v_y分别代表农机的纵向速度和侧向速度；

B：由路径生成模块处获取农机期望路径，采用曲线坐标系，建立误差动力学模型，其中，表示质心CG到期望的路径最近点M的距离，即在期望路径上CG的正交投影点，将其定义为横向偏差；ψ表示实际航向角ψ_r和期望路径的切线方向ψ_d之间的航向误差，可以得到：

ψ＝ψ_r-ψ_d,

S代表起点到当前位置的曲线坐标，ρ(s)代表期望路径在当前点的曲率，则

其中，v_x和v_y分别代表农机的纵向速度和侧向速度，于是，农机的侧向误差和航向角误差可以被定义为

路径跟踪模型可以表示为

选取状态量为选取农机的前轮转角δ_f为控制量，即u(t)＝δ_f，则上述车辆跟踪模型可以被表示为

C：将上述求得的车辆跟踪模型线性化、离散化处理；规划决策控制模块生成的点，每一点都符合农机跟踪模型方程；参考系统的任意时刻的状态和控制量满足以下关系：

在任意点(ξ_r,u_r)处进行泰勒展开，只保留一阶项，得到：

将上述两式相减得到式中A(t)＝J_f(ξ),B(t)＝J_f(u)，

这样得到的方程线性连续的，再将其离散化；

A_k,t＝I+TA(t)

B_k,t＝TB(t)，由此可得

D：将上述方程转化，建立模型预测方程；

设定得到新的状态空间表达式

其中

然后预测时域内的输出可以推导得到Y(t)＝ψ_tξ(t|t)+Θ△U(t)，

其中

E：设定优化函数并转化为二次型求解；

选取优化函数

其中，N_p为控制时域，N_c为控制时域，Q表示为预测时域的权重矩阵,R为控制时域的权重矩阵，ρ为权重系数，ε为松弛因子，i表示当前时刻后的第i步；

将上述优化函数转化为标准二次型

J(ξ(t),u(t-1),△U(t))＝[△U(t)^T,ε]^TH_t[△U(t)^T,ε]+G_t[△U(t)^T,ε]+P_t，其中，G_t＝[2E(t)^TQΘ_t 0],P_t＝E(t)^TQE(t)Y_ref(t)＝[η_ref(t+1|t),…,η_ref(t+N_p|t)]^T；

min J(ξ(t),u(t-1),△U(t))

s.t.△U_min≤△U_t≤△U_max

U_min≤A△U_t+U_t≤U_max

y_hc.min≤y_hc≤y_hc.max

y_sc.min-ε≤y_sc≤y_sc.min+ε

求解二次型问题，ε>0，其中，y_hc和y_sc分别代表硬约束和软约束；

F：在每个周期内求解二次型问题，得到农机的当前最优控制序列将该序列第一个元素作为实际控制量，即可得到前轮转角为u(t)＝u(t-1)+△u_t ^*，由此即可得到决策控制模块的实际输出；

作为本发明的进一步优选，执行模块与决策控制模块通过串口传输数据；

作为本发明的进一步优选，执行模块接收到决策控制模块的信号为PWM信号，从而控制步进电机转动角度；

作为本发明的进一步优选，紧急控制模块通过控制直线推杆，挤压制动主缸，实现车辆的制动。

通过以上技术方案，相对于现有技术，本发明具有以下有益效果：

为了解决现有的智能农机装备跟踪精度差，控制系统复杂，适应性差，系统可靠性不足的缺点，本发明公开了一种在硬约束和软约束条件下，基于农机动力学模型的利用MPC理论的自动驾驶农机轨迹跟踪系统及控制方法；利用MPC理论预测未来时刻输出量，计算得到当前最优控制序列，将第一个控制量输入执行模块，执行模块执行农机转向，在下一个周期内重复上述过程，直到路径跟踪结束；本发明可以适应农机不同作业场景，避免了高速条件下跟踪误差偏大的问题，确保了跟踪精度以及稳定性，同时整体控制框架简单，鲁棒性强，适合农业机械使用。

附图说明

下面结合附图和实施例对本发明进一步说明。

图1是本发明的优选实施例的总体框架图；

图2为本发明的优选实施例的MPC的原理图；

图3为本发明的优选实施例的智能农机动力学模型；

图4为本发明的优选实施例采用的路径跟踪误差模型示意图。

具体实施方式

现在结合附图对本发明作进一步详细的说明。这些附图均为简化的示意图，仅以示意方式说明本发明的基本结构，因此其仅显示与本发明有关的构成。

如图1所示，本发明的一种基于MPC的自动驾驶农机轨迹跟踪系统，包括用于采集农机周围信息的环境感知模块、用于用户定义路径并生成离散参考路径点的路径生成模块、用于将收集到的信号进行计算的决策控制模块、用于农机出现故障时实现紧急制动的紧急控制模块以及实现车辆运行及停止的执行模块；

前述的环境感知模块采集农机的位置、航向角等信息，并将收集到的信息传输至决策控制模块处，路径生成模块用于用户定义路径并生成离散参考路径点后传输至决策控制模块处，决策控制模块将收集到的信号进行计算，获取当前农机的路径及期望路径偏差，计算出转角控制量，紧急制动模块用于农机在出现故障时向执行模块发出指令实现紧急制动，执行模块从上级接收转角以及制动信号，实现车辆转向及减速停止；

作为本发明的进一步优选，前述的路径生成模块包含显示屏和路径生成控制器；环境感知模块包括安装在智能农机内的组合导航系统和转角传感器，紧急制动模块包括继电器，执行模块包括步进电机和制动系统；

一种基于MPC的自动驾驶农机轨迹跟踪方法，包括以下运行步骤：

第一步：路径生成模块通过CAN总线将将生成的离散期望轨迹点传递给决策控制模块；

第二步：环境感知模块通过串口将农机的位置、航向、前轮实际转角信息传递给决策控制模块；

第三步：决策控制模块由环境感知模块处获取的信息计算当前状态量，决策控制模块由路径生成模块处获取的信息计算期望状态的误差，得出最优控制量，并将控制量的实际输出传递给执行模块；

第四步：执行模块通过驱动步进电机带动农机转向，在下一个周期内继续重复上述过程，不断减小与参考轨迹的误差，直到跟踪结束；

第五步：如遇紧急情况，紧急控制模块驱动执行模块进行紧急制动操作；

图2所示，基于MPC框架图，作为本发明的进一步优选，前述决策控制模块的处理过程包括以下步骤：

图3所示，A：环境感知模块获取农机实时位置信息，结合农机结构参数，通过二自由度模型理论，进行动力学建模，具体参数为

其中：

其中，定义CG为农机的质心，γ为农机的横摆角速度，β为质心侧偏角，δ_f为前轮转角，C_f为前轮侧偏刚度，C_r为后轮侧偏刚度，I_z为绕通过其质心CG的垂直轴线转动惯量，前后轮到CG距离为l_f和l_r，v_x和v_y分别代表农机的纵向速度和侧向速度；

B：由路径生成模块处获取农机期望路径，采用曲线坐标系，建立误差动力学模型，其中，图4所示，表示质心CG到期望的路径最近点M的距离，即在期望路径上CG的正交投影点，将其定义为横向偏差；ψ表示实际航向角ψ_r和期望路径的切线方向ψ_d之间的航向误差，可以得到：

ψ＝ψ_r-ψ_d,

S代表起点到当前位置的曲线坐标，ρ(s)代表期望路径在当前点的曲率，则

其中，v_x和v_y分别代表农机的纵向速度和侧向速度，于是，农机的侧向误差和航向角误差可以被定义为

路径跟踪模型可以表示为

选取状态量为选取农机的前轮转角δ_f为控制量，即u(t)＝δ_f，则上述车辆跟踪模型可以被表示为

C：将上述求得的车辆跟踪模型线性化、离散化处理；规划决策控制模块生成的点，每一点都符合农机跟踪模型方程；参考系统的任意时刻的状态和控制量满足以下关系：

在任意点(ξ_r,u_r)处进行泰勒展开，只保留一阶项，得到：

将上述两式相减得到式中A(t)＝J_f(ξ),B(t)＝J_f(u)，

这样得到的方程线性连续的，再将其离散化；

A_k,t＝I+TA(t)

B_k,t＝TB(t)，由此可得

D：将上述方程转化，建立模型预测方程；

设定得到新的状态空间表达式

其中

然后预测时域内的输出可以推导得到Y(t)＝ψ_tξ(t|t)+Θ△U(t)，

其中

E：设定优化函数并转化为二次型求解；

选取优化函数

其中，N_p为控制时域，N_c为控制时域，Q表示为预测时域的权重矩阵,R为控制时域的权重矩阵，ρ为权重系数，ε为松弛因子，i表示当前时刻后的第i步；

将上述优化函数转化为标准二次型

J(ξ(t),u(t-1),△U(t))＝[△U(t)^T,ε]^TH_t[△U(t)^T,ε]+G_t[△U(t)^T,ε]+P_t，其中，G_t＝[2E(t)^TQΘ_t 0],P_t＝E(t)^TQE(t)Y_ref(t)＝[η_ref(t+1|t),…,η_ref(t+N_p|t)]^T；

min J(ξ(t),u(t-1),△U(t))

s.t.△U_min≤△U_t≤△U_max

U_min≤A△U_t+U_t≤U_max

y_hc.min≤y_hc≤y_hc.max

y_sc.min-ε≤y_sc≤y_sc.min+ε

求解二次型问题，ε>0，其中，y_hc和y_sc分别代表硬约束和软约束；

F：在每个周期内求解二次型问题，得到农机的当前最优控制序列将该序列第一个元素作为实际控制量，即可得到前轮转角为u(t)＝u(t-1)+△u_t ^*，由此即可得到决策控制模块的实际输出；

作为本发明的进一步优选，执行模块与决策控制模块通过串口传输数据；

作为本发明的进一步优选，执行模块接收到决策控制模块的信号为PWM信号，从而控制步进电机转动角度；

作为本发明的进一步优选，紧急控制模块通过控制直线推杆，挤压制动主缸，实现车辆的制动。

本发明所提出的建立车辆动力学方程及曲线坐标系误差方程，基于MPC的控制器设计方法，可以适应农机不同作业场景，避免了高速条件下跟踪误差偏大的问题，确保了跟踪精度以及稳定性，同时整体控制框架简单，鲁棒性强，适合农业机械使用。

本技术领域技术人员可以理解，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)具有与本申请所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样定义，不会用理想化或过于正式的含义来解释。

本申请中所述的“和/或”的含义指的是各自单独存在或两者同时存在的情况均包括在内。

本申请中所述的“连接”的含义可以是部件之间的直接连接也可以是部件间通过其它部件的间接连接。

以上述依据本发明的理想实施例为启示，通过上述的说明内容，相关工作人员完全可以在不偏离本项发明技术思想的范围内，进行多样的变更以及修改。本项发明的技术性范围并不局限于说明书上的内容，必须要根据权利要求范围来确定其技术性范围。

Claims (7)

1.一种基于MPC的自动驾驶农机轨迹跟踪系统，其特征在于：包括用于采集农机周围信息的环境感知模块、用于用户定义路径并生成离散参考路径点的路径生成模块、用于将收集到的信号进行计算的决策控制模块、用于农机出现故障时实现紧急制动的紧急控制模块以及实现车辆运行及停止的执行模块；

前述的环境感知模块采集农机的位置、航向角等信息，并将收集到的信息传输至决策控制模块处，路径生成模块用于用户定义路径并生成离散参考路径点后传输至决策控制模块处，决策控制模块将收集到的信号进行计算，获取当前农机的路径及期望路径偏差，计算出转角控制量，紧急制动模块用于农机在出现故障时向执行模块发出指令实现紧急制动，执行模块从上级接收转角以及制动信号，实现车辆转向及减速停止。

2.根据权利要求1所述的基于MPC的自动驾驶农机轨迹跟踪系统，其特征在于：前述的路径生成模块包含显示屏和路径生成控制器；环境感知模块包括安装在智能农机内的组合导航系统和转角传感器，紧急制动模块包括继电器，执行模块包括步进电机和制动系统。

3.一种基于MPC的自动驾驶农机轨迹跟踪方法，其特征在于：包括以下运行步骤：

第一步：路径生成模块通过CAN总线将将生成的离散期望轨迹点传递给决策控制模块；

第二步：环境感知模块通过串口将农机的位置、航向、前轮实际转角信息传递给决策控制模块；

第三步：决策控制模块由环境感知模块处获取的信息计算当前状态量，决策控制模块由路径生成模块处获取的信息计算期望状态的误差，得出最优控制量，并将控制量的实际输出传递给执行模块；

第四步：执行模块通过驱动步进电机带动农机转向，在下一个周期内继续重复上述过程，不断减小与参考轨迹的误差，直到跟踪结束；

第五步：如遇紧急情况，紧急控制模块驱动执行模块进行紧急制动操作。

4.根据权利要求3所述的基于MPC的自动驾驶农机轨迹跟踪方法，其特征在于：前述决策控制模块的处理过程包括以下步骤：

A：环境感知模块获取农机实时位置信息，结合农机结构参数，通过二自由度模型理论，

进行动力学建模，具体参数为

其中：

其中，定义CG为农机的质心，γ为农机的横摆角速度，β为质心侧偏角，δ_f为前轮转角，C_f为前轮侧偏刚度，C_r为后轮侧偏刚度，I_z为绕通过其质心CG的垂直轴线转动惯量，前后轮到CG距离为l_f和l_r，v_x和v_y分别代表农机的纵向速度和侧向速度；

B：由路径生成模块处获取农机期望路径，采用曲线坐标系，建立误差动力学模型，其中，表示质心CG到期望的路径最近点M的距离，即在期望路径上CG的正交投影点，将其定义为横向偏差；ψ表示实际航向角ψ_r和期望路径的切线方向ψ_d之间的航向误差，可以得到：

ψ＝ψ_r-ψ_d,

S代表起点到当前位置的曲线坐标，ρ(s)代表期望路径在当前点的曲率，则

其中，v_x和v_y分别代表农机的纵向速度和侧向速度，于是，农机的侧向误差和航向角误差可以被定义为

路径跟踪模型可以表示为

选取状态量为选取农机的前轮转角δ_f为控制量，即u(t)＝δ_f，则上述车辆跟踪模型可以被表示为

C：将上述求得的车辆跟踪模型线性化、离散化处理；规划决策控制模块生成的点，每一点都符合农机跟踪模型方程；参考系统的任意时刻的状态和控制量满足以下关系：

在任意点(ξ_r,u_r)处进行泰勒展开，只保留一阶项，得到：

将上述两式相减得到式中A(t)＝J_f(ξ),B(t)＝J_f(u)，

这样得到的方程线性连续的，再将其离散化；

由此可得

D：将上述方程转化，建立模型预测方程；

设定得到新的状态空间表达式

其中

然后预测时域内的输出可以推导得到Y(t)＝ψ_tξ(t|t)+Θ△U(t)，

其中

E：设定优化函数并转化为二次型求解；

选取优化函数

其中，N_p为控制时域，N_c为控制时域，Q表示为预测时域的权重矩阵,R为控制时域的权重矩阵，ρ为权重系数，ε为松弛因子，i表示当前时刻后的第i步；

将上述优化函数转化为标准二次型

J(ξ(t),u(t-1),△U(t))＝[△U(t)^T,ε]^TH_t[△U(t)^T,ε]+G_t[△U(t)^T,ε]+P_t，其中，

G_t＝[2E(t)^TQΘ_t 0],P_t＝E(t)^TQE(t)

求解二次型问题，其中，y_hc和y_sc分别代表硬约束和软约束；

F：在每个周期内求解二次型问题，得到农机的当前最优控制序列将该序列第一个元素作为实际控制量，即可得到前轮转角为u(t)＝u(t-1)+△u_t ^*，由此即可得到决策控制模块的实际输出。

5.根据权利要求3所述的基于MPC的自动驾驶农机轨迹跟踪方法，其特征在于：执行模块与决策控制模块通过串口传输数据。

6.根据权利要求3所述的基于MPC的自动驾驶农机轨迹跟踪方法，其特征在于：执行模块接收到决策控制模块的信号为PWM信号，从而控制步进电机转动角度。

7.根据权利要求3所述的基于MPC的自动驾驶农机轨迹跟踪方法，其特征在于：紧急控制模块通过控制直线推杆，挤压制动主缸，实现车辆的制动。