CN113885518B - 基于信任域的井下矿用铰接车轨迹规划方法 - Google Patents

Abstract

本发明公开了一种基于信任域的井下矿用铰接车轨迹规划方法，包括：生成一条粗略的行车轨迹：在明确了行驶的起点和终点前提下，形成一条衔接始末位置的避障轨迹；基于信任域技术求解精细的最优的运动轨迹：构建一个最优控制问题描述轨迹规划任务，随后对其进行数值求解，数值求解的过程中引入信任域技术辅助加速计算。本发明为井下采矿场景中多体铰接车的轨迹规划方法提供了明确的现实意义，在保障求解结果最优的前提下提升了计算效率。

Description

基于信任域的井下矿用铰接车轨迹规划方法

技术领域

本发明涉及井下矿用铰接车技术领域，尤其涉及一种基于信任域的井下矿用铰接车轨迹规划方法。

背景技术

井下环境往往是极其狭窄的隧道，因此对车辆的运动规划方法提出了非常高的要求。矿用铰接车属于多体车辆，而普通轿车属于刚体。铰接车的复杂性体现在，它的动力全部来源于车头的拖车部分，而后面悬挂的诸多挂车都是被动受力运动，因此铰接车属于典型的欠驱动系统，一般难以规划开环轨迹或执行闭环控制。

现有的适合多体铰接车的轨迹规划方法包括采样搜索方法以及数值最优控制方法，前者的搜索空间较大，受限于空间分辨率，因此求解成功率以及所得到的结果的最优性都存在理论上的隐患；后者能够准确描述运动规划任务，且保证得到高精度最优解，但是求解缓慢。

发明内容

本发明所要解决的技术问题在于，针对现有的适合多体铰接车的轨迹规划方法，求解成功率以及所得到的结果的最优性都存在理论上的隐患，以及求解缓慢的问题，提出了一种基于信任域的井下矿用铰接车轨迹规划方法。

本基于信任域的井下矿用铰接车轨迹规划方法包括：

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

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

其中，步骤一中，行车轨迹的具体规划方法为：采用A*算法规划车头的路径PATH；利用庞特里亚金极值原理沿着既定路径PATH规划速度，从而形成时间最优的车头轨迹；利用铰接车运动学关联关系确定剩余挂车的行驶轨迹。

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

拖挂车是由一节拖车头和若干节挂车顺次铰接而成的多体车辆，为拖挂车的各组成单元分配唯一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∈[0，t_f].

其中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)可由下式确定：

i＝1,...,N_V.

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

其中，buffer取值0.1rad；

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

i＝1,...,N_V.

在拖挂车运动的起始时刻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),o_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),A_r(t)B_r(t)C_r(t)D_r(t)),

t∈[0,t_f],j,r＝0,...,N_V,r≠j；

其中，o_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]上，即：

0＝t₀＜t₁＜t₂＜...＜t_Nfe＝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(χ)＜0，

h(χ)＝0.

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

min J(χ，s)，

s.t.g(χ)+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的精准程度越高。

其中，步骤二中，数值求解的过程中引入信任域技术辅助加速计算的方法为：

首先，利用第一步得到的拖车和挂车们的轨迹，将其记为Traj(j)，j＝0，...，N_v，在最优控制问题中加入以下信任域约束：

|x_j(t)-Traj(j).x(t)|≤Δs

|y_j(t)-Traj(j).y(t)|≤Δs

|θ_j(t)-Traj(j).θ(t)|≤Δa

j＝0,...,N_V

式中的Δs,Δa＞0是由用户评经验指定的参数，分别取值为0.5–2.0m和0.2–0.5rad，由于x_j(t)、y_j(t)以及θ_j(t)在受限的信任域区间上，因此车辆位置姿态不会与环境中的所有障碍物相撞，采取蒙特卡洛方法对上述区间对应的位置姿态进行随机组合，然后估算车辆的足迹，接着检验这些随机足迹是否与障碍物碰撞，这样的随机采样可进行N次，如果经历N次检验，都未能发生碰撞的障碍物，我们从前述碰撞躲避约束公式之中将其中未涉及碰撞的障碍物去除，从而简化最优控制问题之中的约束条件规模。

其中，附件C为：

假设X-Y坐标系中存在N_OBS个静止的凸多边形障碍物，其中第r个障碍物包含NV_r个顶点V_r1～V_rNPr，将针对车身与凸多边形障碍物r之间的碰撞躲避约束进行建模，在二维平面上，两个凸多边形从尚未发生碰撞到发生碰撞的整个过程中，碰撞发生的瞬间一定出现了某一凸多边形顶点撞入了另一凸多边形中的事件，点P处于凸多边形Q₁～Q_n外部的约束条件可正式建立为：

其中S_Δ代表相应三角形面积，S_□代表凸多边形面积，S_Δ应基于三角形顶点坐标书写，以为例，假设P＝(x,y)、Q_k＝(x_Qk,y_Qk)、Q_k+1＝(x_Q(k+1),y_Q(k+1))，则有：

其中是常值；

将点P处于凸多边形Q₁～Q_n外部的一般性约束条件简记为PointOutOfPolygon(P,Q₁...Q_n)，据此可建立第r个障碍物与车身矩形A(t)B(t)C(t)D(t)的碰撞躲避约束条件，完整的碰撞躲避约束条件可写为：

r＝1,...,N_OBS,t∈[0,t_f]

为叙述方便，将上式进一步简写为：

实施本发明实施例，具有如下有益效果：

本发明为井下采矿场景中多体铰接车的轨迹规划方法提供了明确的现实意义，在保障求解结果最优的前提下提升了计算效率。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为拖挂车运动学模型；

图2为描述点P位于凸多边形Q1Q2Q3Q4Q5外部的三角形面积判据示意图。

具体实施方式

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

本基于信任域的井下矿用铰接车轨迹规划方法包括：

步骤一、生成一条粗略的行车轨迹：在明确了行驶的起点和终点前提下，形成一条衔接始末位置的避障轨迹。行车轨迹的具体规划方法为：采用A*算法规划车头的路径PATH；利用庞特里亚金极值原理沿着既定路径PATH规划速度，从而形成时间最优的车头轨迹；利用铰接车运动学关联关系确定剩余挂车的行驶轨迹。

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

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

拖挂车是由一节拖车头和若干节挂车顺次铰接而成的多体车辆，为拖挂车的各组成单元分配唯一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，L_Ni、L_Wi、L_Mi等其他几何参数在图1中标出，请参见图1，图1为拖挂车运动学模型。H_j是指固定于第j个单元上的铰接点(j＝0，..，N_V)，线段P_jH_j的长度是固定的，反映了铰接点与轮轴点的距离；在拖挂车中，如果所有P_jH_j长度均为0，即所有P_j、H_j点重合，则拖挂车称为StandardN-trailer车辆，简称SNT；如果所有P_jH_j长度均非0，则拖挂车称为non-StandardN-trailer车辆，即nSNT；如果拖挂车中部分P_jH_j长度为0，而另一部分长度非0，则拖挂车称为GeneralN-trailer车辆，即GNT。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∈[0，t_f].

其中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)可由下式确定：

i＝1,...,N_V.

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

其中buffer＞0为防范弯折现象的缓冲裕度。评经验buffer取值0.1rad左右；

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

i＝1,...,N_V.

上述所有式子构成了拖挂车系统的运动学约束条件。

在拖挂车运动的起始时刻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),o_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),A_r(t)B_r(t)C_r(t)D_r(t)),

t∈[0,t_f],j,r＝0,...,N_V,r≠j；

其中，o_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)代表控制变量，所有约束条件分为微分方程等式以及代数等式/不等式，这是典型的Bolza型最优控制问题。求解命题上述最优控制问题即确定符合约束条件的控制变量u(t)以及时域长度t_f，使得代价函数t_f得以最小化；

数值求解最优控制问题的第一步是将最优控制问题转化为NLP问题，实现方式是将连续的时间变量t用时域[0,t_f]上的有限个采样时刻{t_k}来表征，从而将确定u(t)的任务转化为确定u(t)在有限个采样时刻{t_k}上取值的新任务。

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

0＝t₀＜t₁＜t₂＜...＜t_Nfe＝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_ｋ-1，u_k-1)·(t_f/N_fe)，k＝1，...，N_fe；

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

求解该NLP问题，即求解各采样时刻上的配置点x_k、u_k以及t_f，在约束条件严格满足的前提下将代价函数最小化。需要再次强调的是，在NLP问题的求解过程中，x_k、u_k以及t_f被视为地位相等的优化变量，它们将被同时求解出来

若一个数学规划问题的代价函数或约束条件中包含非线性函数，则称其为非线性规划问题，即NLP问题。至于本发明中的NLP问题，它所包含的约束条件具有强非凸非线性，因此属于普通的NLP问题。适合处理一般NLP问题的方法是内点算法(interior-pointmethod，IPM)。内点算法隶属于罚函数法。罚函数法的实现思路是将有约束NLP问题转化为一系列无约束问题进行序贯求解，罚函数法可进一步分为外罚函数法与内罚函数法这两个子类别。外罚函数法将原始NLP问题中的硬性约束条件软化为外罚函数多项式而补入代价函数中，违背原始约束条件的解向量会使得外罚函数多项式取值较大，且违背程度越高则取值越大；至于未违背任何原始约束条件的解向量，其外罚函数多项式取值恒为0。通过在代价函数中增补外罚函数多项式，序贯计算过程将被迫逐步向使外罚函数多项式取值较小的方向迁移，即向着解空间可行域迁移。内罚函数法也称障碍罚函数法，该方法将原始NLP问题中的约束条件改造为内罚函数多项式而补入代价函数中。与外罚函数法相比，内罚函数法要求序贯求解过程始终在解空间可行域内部进行，如某一解向量远离可行域边界则内罚函数取值较小，否则内罚函数值会随着解向量逼近可行域边缘而趋于无穷大。IPM是一种典型的内罚函数方法，以下为IPM的基本实现过程。

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

s.t.g(χ)＜0,

h(χ)＝0.

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

min J(χ,s),

s.t.g(χ)+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的精准程度越高。IPM算法求解命题的方式实质上是序贯求解μ_IPM取值逐步趋于0⁺所对应的一系列形如上式的子问题。随着μ_IPM趋于0⁺，子问题的极值将趋于NLP问题的原命题的极值。由以上分析可知IPM算法通过双层循环架构求解NLP命题：在外层循环更新μ_IPM，在内层迭代求解给定μ_IPM条件下的子问题。

三、数值求解的过程中引入信任域技术辅助加速计算的方法为：

首先，利用第一步得到的拖车和挂车们的轨迹，将其记为Traj(j),j＝0,...,N_V，这些轨迹仅仅起到参考的作用，它们由A*直接或间接得到，因此轨迹的平顺性比较差。在最优控制问题中加入以下信任域约束：

|x_j(t)-Traj(j).x(t)|≤Δs

|y_j(t)-Traj(j).y(t)|≤Δs

|θ_j(t)-Traj(j).θ(t)|≤Δa

j＝0,...,N_V

式中的Δs,Δa＞0是由用户评经验指定的参数，分别取值为0.5–2.0m和0.2–0.5rad，由于x_j(t)、y_j(t)以及θ_j(t)在受限的信任域区间上，因此车辆位置姿态不会与环境中的所有障碍物相撞，采取蒙特卡洛方法对上述区间对应的位置姿态进行随机组合，然后估算车辆的足迹，接着检验这些随机足迹是否与障碍物碰撞，这样的随机采样可进行N次，N是一个很大的数值(可以取10000)。如果经历N次检验，都未能发生碰撞的障碍物，我们从前述碰撞躲避约束公式之中将其中未涉及碰撞的障碍物去除，从而简化最优控制问题之中的约束条件规模。

附件C为：

在建立碰撞躲避约束时，首先应确定障碍物的表达形式。最常见的平面障碍物描述方式是凸多边形，即激光雷达反射点云的最小凸包。假设X-Y坐标系中存在N_OBS个静止的凸多边形障碍物，其中第r个障碍物包含NV_r个顶点将针对车身与凸多边形障碍物r之间的碰撞躲避约束进行建模，在二维平面上，两个凸多边形从尚未发生碰撞到发生碰撞的整个过程中，碰撞发生的瞬间一定出现了某一凸多边形顶点撞入了另一凸多边形中的事件，因此，如果能够在每一时刻都禁止车身矩形顶点落入障碍物内部，并且禁止障碍物顶点落入车辆内部，则碰撞一定不会萌生，障碍物顶点/>车身顶点A(t)～D(t)所在位置均可解析表示，另外还需要一个能够描述“某点处于某一凸多边形外部”的约束条件建模方式，现将这一问题抽象出来，专门考虑如何描述点P＝(x,y)位于具有n个顶点的凸多边形Q₁～Q_n外部。

在计算机图形学中，适合检验点与多边形位置关系的判据有很多种，但是它们均引入形如if else的复杂判据，这些判据适用于判断具体的某一点是否在具体某一凸多边形内部，但不适合构建通用的约束条件。一旦在最优控制问题中显示地掺入此类if else形式的语句，实质上相当于引入了函数导数不连续的隐患，往往将为最优控制命题的求解带来巨大困难。一种相对直观且适合建模的方案是笔者最早使用的所谓“三角面积法”：将点P与凸多边形每两个相邻顶点分别组成三角形，并将这些三角形的面积累加，如果面积之和大于凸多边形，则点P处于凸多边形外部，否则点P处在多边形的轮廓上或内部——这一判断准则的正确性通过初等数学知识即可证明。请参见图2，图2为描述点P位于凸多边形Q1Q2Q3Q4Q5外部的三角形面积判据示意图。

据此，点P处于凸多边形Q₁～Q_n外部的约束条件可正式建立为：

其中S_Δ代表相应三角形面积，S_□代表凸多边形面积，S_Δ应基于三角形顶点坐标书写，以为例，假设P＝(x,y)、Q_k＝(x_Qk,y_Qk)、Q_k+1＝(x_Q(k+1),y_Q(k+1))，则有：

其中是常值；

将点P处于凸多边形Q₁～Q_n外部的一般性约束条件简记为PointOutOfPolygon(P,Q₁...Q_n)，据此可建立第r个障碍物与车身矩形A(t)B(t)C(t)D(t)的碰撞躲避约束条件，完整的碰撞躲避约束条件可写为：

r＝1,...,N_OBS,t∈[0,t_f]

为叙述方便，将上式进一步简写为：

使用本发明的技术方案，在一些典型场景中，使用信任域技术与不使用相比，求解耗时降低了900％。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

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的精准程度越高。

2.根据权利要求1所述的基于信任域的井下矿用铰接车轨迹规划方法，其特征在于，所述步骤一中，行车轨迹的具体规划方法为：采用A*算法规划车头的路径PATH；利用庞特里亚金极值原理沿着既定路径PATH规划速度，从而形成时间最优的车头轨迹；利用铰接车运动学关联关系确定剩余挂车的行驶轨迹。

3.根据权利要求1所述的基于信任域的井下矿用铰接车轨迹规划方法，其特征在于，所述步骤二中，数值求解的过程中引入信任域技术辅助加速计算的方法为：

首先，利用第一步得到的拖车和挂车们的轨迹，将其记为Traj(j),j＝0,...,N_V，在最优控制问题中加入以下信任域约束：

|x_j(t)-Traj(j).x(t)|≤Δs

|y_j(t)-Traj(j).y(t)|≤Δs

|θ_j(t)-Traj(j).θ(t)|≤Δa

j＝0,...,N_V

式中的Δs,Δa＞0是由用户评经验指定的参数，分别取值为0.5–2.0m和0.2–0.5rad，由于x_j(t)、y_j(t)以及θ_j(t)在受限的信任域区间上，因此车辆位置姿态不会与环境中的所有障碍物相撞，采取蒙特卡洛方法对上述区间对应的位置姿态进行随机组合，然后估算车辆的足迹，接着检验这些随机足迹是否与障碍物碰撞，这样的随机采样可进行N次，如果经历N次检验，都未能发生碰撞的障碍物，从前述碰撞躲避约束公式之中将其中未涉及碰撞的障碍物去除，从而简化最优控制问题之中的约束条件规模。

4.根据权利要求1所述的基于信任域的井下矿用铰接车轨迹规划方法，其特征在于，所述附件C为：

假设X-Y坐标系中存在N_OBS个静止的凸多边形障碍物，其中第r个障碍物包含NV_r个顶点将针对车身与凸多边形障碍物r之间的碰撞躲避约束进行建模，在二维平面上，两个凸多边形从尚未发生碰撞到发生碰撞的整个过程中，碰撞发生的瞬间一定出现了某一凸多边形顶点撞入了另一凸多边形中的事件，点P处于凸多边形Q₁～Q_n外部的约束条件可正式建立为：

其中S_Δ代表相应三角形面积，代表凸多边形面积，S_Δ应基于三角形顶点坐标书写，以为例，假设P＝(x,y)、Q_k＝(x_Qk,y_Qk)、Q_k+1＝(x_Q(k+1),y_Q(k+1))，则有：

其中是常值；

将点P处于凸多边形Q₁～Q_n外部的一般性约束条件简记为PointOutOfPolygon(P,Q₁...Q_n)，据此可建立第r个障碍物与车身矩形A(t)B(t)C(t)D(t)的碰撞躲避约束条件，完整的碰撞躲避约束条件可写为：

r＝1,...,N_OBS,t∈[0,t_f]

为叙述方便，将上式进一步简写为：