[发明专利]一种半挂车的避障控制方法

权利要求书

1.一种半挂车的避障控制方法，其特征在于，包括：

建立半挂车的运动学模型；

根据建立的运动学模型预测未来一段时间内半挂车的位姿信息；

建立半挂车的中心线避障模型；

根据预测得到的位姿信息和建立的中心线避障模型，以最小化控制过程中的路径跟踪误差、控制输出变化量和使半挂车远离障碍物为目标，确定非线性模型预测控制的优化函数，得到半挂车的最优控制序列，以便所述半挂车完成对障碍物的避让：

其中，建立的半挂车的运动学模型表示为：

其中，表示牵引车航向角速度；v_f表示牵引车纵向行驶速度；δ表示牵引车前轮转向角；l_fb表示牵引车轴距；表示挂车航向角速度；γ表示牵引车与挂车间角度；l_rb表示铰接点与挂车后桥间的距离；θ_f表示牵引车航向角；θ_r表示挂车航向角；表示牵引车等效后轴L_fr中点坐标在全局坐标系(X，Y)下X方向速度分量；表示牵引车等效后轴L_fr中点坐标在全局坐标系(X，Y)下Y方向速度分量；表示牵引车前端H_f中点在全局坐标系(X，Y)下X方向速度分量；l_fa表示前桥与牵引车前端H_f间的距离；表示牵引车前端H_f中点在全局坐标系(X，Y)下Y方向速度分量；表示牵引车后端T_f中点在全局坐标系(X，Y)下X方向速度分量；l_fc表示牵引车后桥与后端间的距离；表示牵引车后端T_f中点在全局坐标系(X，Y)下Y方向速度分量；表示挂车前端H_r中点在全局坐标系(X，Y)下X方向速度分量；l_ra表示铰接点与挂车前端H_r间的距离；表示挂车前端H_r中点在全局坐标系(X，Y)下Y方向速度分量；表示挂车后端T_r中点在全局坐标系(X，Y)下X方向速度分量；l_rc表示挂车后桥与后端之间的距离；表示挂车后端T_r中点在全局坐标系(X，Y)下Y方向速度分量；

其中，所述根据建立的运动学模型预测未来一段时间内半挂车的位姿信息包括：

确定半挂车的运动学模型的状态量x与控制量u；其中，状态量x＝[θ_f θ_r γ x_f y_f x_ffy_ff x_fr y_fr x_rf y_rf x_rr y_rr]^T，其中，x_f表示牵引车等效后轴L_fr中点在全局坐标系(X，Y)下X方向的坐标；y_f表示牵引车等效后轴L_fr中点在全局坐标系(X，Y)下Y方向的坐标；x_ff表示牵引车前端H_f中点在全局坐标系(X，Y)下X方向的坐标；y_ff表示牵引车前端H_f中点在全局坐标系(X，Y)下Y方向的坐标；x_fr表示牵引车后端T_f中点在全局坐标系(X，Y)下X方向的坐标；y_fr表示牵引车后端T_f中点在全局坐标系(X，Y)下Y方向的坐标；x_rf表示挂车前端H_r中点在全局坐标系(X，Y)下X方向的坐标；y_rf表示挂车前端H_r中点在全局坐标系(X，Y)下Y方向的坐标；x_rr表示挂车后端T_r中点在全局坐标系(X，Y)下X方向的坐标；y_rr表示挂车后端T_r中点在全局坐标系(X，Y)下Y方向的坐标；控制量u＝[δ v_f]^T；

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

对建立的运动学模型进行离散化，建立预测模型，根据半挂车当前的位姿信息、牵引车质心速度信息，通过预测模型预测未来一段时间[t，t+N_p]内的位姿信息，所述预测模型表示为：

其中，T表示采样周期；x(t+i|t)为t时刻第i个预测点的状态量，i＝0，1，...，N_p；u(t+m|t)为t时刻第m个预测点的控制量，m＝0，1，...，N_p-1；N_p为预测时域，N_c表示控制时域；

其中，所述建立半挂车的中心线避障模型包括：

确定预测时域内第j个障碍物到牵引车前端H_f、牵引车后端T_f、挂车前端H_r和挂车后端T_r的距离分别为：

其中，d_ffj(t+i|t)、d_frj(t+i|t)、d_rfj(t+i|t)、d_rrj(t+i|t)分别表示预测时域内第j个障碍物到牵引车前端H_f、牵引车后端T_f、挂车前端H_r、挂车后端T_r的距离；x_zj、y_zj分别表示第j个障碍物的横、纵坐标；

根据半挂车的中心线模型和得到的预测时域内第j个障碍物到牵引车前端H_f、牵引车后端T_f、挂车前端H_r和挂车后端T_r的距离，判断障碍物O是否位于牵引车车侧方区域B或挂车车侧方区域E：

其中，B表示牵引车车侧方区域，E表示挂车车侧方区域；

当第j个障碍物位于区域B或者区域E时，第j个障碍物到牵引车中心线和挂车中心线间的垂直距离分别为：

其中，d_fj(t+i|t)、d_rj(t+i|t)分别表示第j个障碍物到牵引车中心线、挂车中心线间的垂直距离，x_f表示挂车等效前轴L_rf中点在全局坐标系(X，Y)下X方向的坐标；y_f表示挂车等效前轴L_rf中点在全局坐标系(X，Y)下Y方向的坐标；

根据半挂车的牵引车避障模型、半挂车的挂车避障模型和得到的第j个障碍物到牵引车中心线和挂车中心线间的垂直距离，建立半挂车的中心线避障模型；

其中，半挂车的中心线模型表示为：

l_l＝l_r+l_s+l_b

其中，S_c表示半挂车的中心线模型；M表示惩罚函数项；l_l表示障碍物到车身中心线的最小安全距离；d₃表示障碍物外接圆圆心到牵引车车身中心线的距离；O表示障碍物；II表示障碍物位于的区域；l_r表示障碍物的外接圆距离；l_s表示安全裕度；l_b表示半挂车车身宽度的一半；

其中，半挂车的牵引车避障模型为：

其中，S_f表示半挂车的牵引车避障模型；

其中，半挂车的挂车避障模型为：

其中，S_r表示半挂车的挂车避障模型，d₆表示障碍物外接圆圆心到挂车车身中心线的距离；

其中，建立的半挂车的中心线避障模型表示为：

其中，S(t+i|t)表示建立的半挂车的中心线避障模型。