[发明专利]基于信任域的井下矿用铰接车轨迹规划方法

权利要求书

1.一种基于信任域的井下矿用铰接车轨迹规划方法，其特征在于，包括以下步骤：

步骤一、生成一条粗略的行车轨迹：在明确了行驶的起点和终点前提下，形成一条衔接始末位置的避障轨迹；

步骤二、基于信任域技术求解精细的最优的运动轨迹：构建一个最优控制问题描述轨迹规划任务，随后对其进行数值求解，数值求解的过程中引入信任域技术辅助加速计算；

其中，构建一个最优控制问题描述轨迹规划任务具体为：

拖挂车是由一节拖车头和若干节挂车顺次铰接而成的多体车辆，为拖挂车的各组成单元分配唯一ID：拖车头的ID为0，第i节挂车的ID为i，i＝1,...,N_V，N_V代表挂车的总数目，则完整的拖挂车包含N_V+1个组成单元；并设，L_N0为拖车前悬长度，L_M0为拖车后悬长度，L_W0为拖车轴距，φ₀为拖车的前轮转向角，P₀＝(x₀,y₀)为拖车后轮轴中心点，每一节挂车参考点可指定为其轮轴中心点P_i＝(x_i,y_i)，L_Wi为第i节挂车参考点P_i与前一单元铰接点H_i-1的距离，i＝1,...,N_V，θ_j为第j部分单元的姿态角，j＝0,...,N_V，H_j是指固定于第j个单元上的铰接点，j＝0,...,N_V，线段P_jH_j的长度是固定的，反映了铰接点与轮轴点的距离；L_H0为从拖车后轮中心点P₀到拖车与挂车1的铰接点H₀之间的这段线段的长度；L_Hi表征的是挂车i的轮轴中心点P_i到挂车i与挂车i+1铰接点H_i之间的距离；L_H(i-1)表征的是挂车i-1的轮轴中心点P_i-1到挂车i-1与挂车i铰接点H_i-1之间的距离；

拖车头在惯性坐标系X-Y中的运动过程受到以下微分方程组的制约：

其中t_f为车辆运动过程的终止时刻，v₀(t)及a₀(t)分别为拖车沿车身纵轴方向的速度及加速度，ω₀(t)为拖车前轮转角的角速度，与拖车相关的变量存在容许作用区间：

-Φ_max≤φ₀(t)≤Φ_max，

v_min≤v₀(t)≤v_max，

a_min≤a₀(t)≤a_max，

-Ω_max≤ω₀(t)≤Ω_max，t∈[0，t_f]；

假设与拖车固定的铰接点则可根据拖车姿态角θ₀以及P₀坐标确定H₀位置：

由挂车1的姿态角θ₁以及H₀坐标可确定P₁点坐标：

可使用上述方法逐一确定所有P_j位置：

x_j+1(t)＝x_j(t)-L_W(j+1)·cosθ_j+1(t)-L_Hj·cosθ_j(t),

y_j+1(t)＝y_j(t)-L_W(j+1)·sinθ_j+1(t)-L_Hj·sinθ_j(t),

j＝0,...,N_V

每一节挂车的姿态角θ_i(t)可由下式确定：

在拖挂车系统中，相邻单元之间的姿态角夹角不宜超过90°，否则车辆系统会陷入弯折状态(jackknife)而难以恢复，因此可设置以下约束条件：

其中buffer取值0.1rad；

每一节挂车的速度v_i(t)由下式确定：

在拖挂车运动的起始时刻t＝0，应显示地指定车辆系统所处的运动状态，即针对变量x₀(0)、y₀(0)、v₀(0)、φ₀(0)、a₀(0)、ω₀(0)、θ_j(0),j＝0,...,N_V赋值，在对上述变量施加点约束后，其余状态变量，x_i(0)、y_i(0)、v_i(0)，i＝1,...,N_V，都可以唯一确定下来；

在终止时刻t_f，拖挂车系统应达到某一既定运动状态，一般要求车辆最终稳定地停泊，因此可建立以下约束条件：

v₀(t_f)＝a₀(t_f)＝φ₀(t_f)＝ω₀(t_f)＝0；

为使拖挂车运动到某一指定区域，可以针对x_j(t_f)、y_j(t_f)，j＝0,...,N_V直接施加点约束，

设拖挂车系统中单元j的四个顶点分别为A_j(t)、B_j(t)、C_j(t)以及D_j(t)，则碰撞躲避约束可写为：

VehicleOutOfPolygon(A_j(t)B_j(t)C_j(t)D_j(t),ο_r(t)),t∈[0,t_f],j＝0,...,N_V,r＝1,...,N_OBS

以及

VehicleOutOfPolygon(A_j(t)B_j(t)C_j(t)D_j(t),Ar(t)Br(t)Cr(t)Dr(t)),t∈[0,t_f],j,r＝0,...,N_V,r≠j；

其中，ο_r(t)记录着第r个障碍物的顶点在t时刻的位置，N_OBS代表障碍物数目，VehicleOutOfPolygon函数的具体写法在附件C给出；

拖挂车的全部动力源于其头部的拖车，因此拖挂车决策规划任务的代价函数设置方式与刚体车辆类似，可设置：J＝t_f；

将上述所列出的约束条件合并起来，即构成完整的用于描述拖挂车轨迹规划任务的最优控制问题；

对构建的最优控制问题进行数值求解，最优控制问题可抽象地简记为以下形式：

min t_f，

G(x(t)，u(t))≤0，t∈[0，t_f]；

其中x(t)代表状态变量，u(t)代表控制变量，求解命题上述最优控制问题即确定符合约束条件的控制变量u(t)以及时域长度t_f，使得代价函数t_f得以最小化；

首先，定义N_fe+1个采样时刻{t_k|k＝0,...,N_fe}，要求这些采样时刻均匀分布于时域0-t_f上，即：

第二步，引入一系列变量{u_k|k＝0,...,N_fe}、{x_k|k＝0,...,N_fe}来分别表征u(t)、x(t)，求解u(t)、x(t)这一原始任务转化为求解它们在一系列采样时刻的取值，即求解变量{u_k}、{x_k}的取值，由于在离散化意义下已不存在连续时间变量t，因此在最优控制问题的原始命题中与t相关的部分均需相应改造，代价函数变为代数等式/不等式G(x(t),u(t))≤0,t∈[0,t_f]变为G(u_k,x_k)≤0，k＝0,...,N_fe，微分等式的变化方式稍微复杂，将dx(t)/dt＝F(x(t),u(t))写以下等价形式：

其中无论变量a如何选取，上式一定成立，现将上式中的t取为t_k，并令a＝t_k-1，则有：

为去除复杂的积分运算，可将被积函数整体近似为一个常值Const，即：

由于函数F(·，·)本身不可能是常值函数，上式的成立意味着x(t)、u(t)在子区间t∈(t_k-1，t_k)上取值恒定，即：

其中Const₁、Const₂为常值，上式可写为：

x(t_k)＝x(t_k-1)+Const·(t_k-t_k-1)，

即：

x_k＝x_k-1+Const·h_k；

上式去除了积分运算以及连续变量t，最后还需明确常值Const究竟应该如何选取，这相当于明确x(t)、u(t)在子区间t∈(t_k-1，t_k)上取什么样的常值，最常见的取值方式是选取子区间端点处的变量值，即：

或者：

以上两式分别代表在子区间左右两端取值，从中选定一种取值方式即可；以在左端取值为例，则可为：

x_k＝x_k-1+F(x_k-1，u_k-1)·h_k；

在所有子区间(t_k-1，t_k)上重复上述转化，则可形成一个NLP问题：

s.t.x_k＝x_k-1+F(x_k-1，u_k-1)·(t_f/N_fe)，k＝1，...，N_fe；

G(u_k，x_k)≤0，k＝0，...，N_fe

首先，待求解的NLP问题可精炼地描述为：

s.t.g(x)＜0，

h(χ)＝0；

其中χ代表由优化变量组成的向量，即解向量，通过引入松弛向量s＞0，可将不等式约束g(χ)＜0转化为等式约束，即：

min J(x，s)，

s.tg(χ)+s＝0，

h(χ)＝0，

s＞0；

此时，如将上式中唯一的不等式约束条件s＞0转化为内罚函数项补入代价函数J，则可构造仅包含等式约束的标准形式NLP问题：

min J_μ(χ，s)＝J(χ)-μ_IPM·ln(s)，

s.t.g(χ)+s＝0，

h(χ)＝0；

其中，J_μ(χ,s)代表包含内罚函数项的代价函数；μ_IPM＞0是障碍因子，其取值越趋于0⁺则通过内罚函数μ_IPM·ln(s)描述不等式s＞0的精准程度越高。