一种双无人车动态轨迹跟踪与避障自适应鲁棒控制方法

1.本发明涉及机械系统动力学以及控制领域，具体涉及一种双无人车动态轨迹跟踪与避障自适应鲁棒控制方法。


背景技术：

2.随着无人平台技术的智能化发展，在执行各项任务的过程中也扮演着十分重要的角色。在现有的研究中，绝大多数对移动控制算法的研究都比较片面，仅考虑对路线的跟踪、路线的规划等问题或是躲避障碍目标的问题。然而在实际情况中，无人车到达指定目标中，轨迹跟踪与避障的控制目标可能是时变且同时发生的，并且由于存在模型误差、测量误差、摩擦以及其他干扰因素，无人车控制过程中面临诸多不确定性因素干扰，因此目前的许多传统控制方法不能满足其性能要求。本发明引入约束跟随理论，设计一种针对双无人车动态轨迹跟踪与避障的自适应鲁棒控制方法，由侦察车负责探测路径以及障碍，由救援车进行轨迹跟踪与避障行为，要求能够有更高的灵活性、更高的控制精度以及在时变不确定性干扰下的优秀鲁棒性。


技术实现要素：

3.本发明的目的在于提供一种双无人车动态轨迹跟踪与避障自适应鲁棒控制方法。
4.实现本发明目的的技术方案为：一种双无人车动态轨迹跟踪与避障自适应鲁棒控制方法，包括以下步骤：
5.步骤1)，基于拉格朗日建模方法，建立侦察车和救援车统一的无人车动力学模型；
6.步骤2)，根据控制目标，建立侦察车所需要的目标到达约束，救援车所需要的轨迹跟踪约束和避障约束的数学模型，并以一阶约束和二阶约束的形式表达；
7.步骤3)，基于约束跟随理论对步骤2)建立的到达约束、轨迹约束和避障约束的数学模型进行分析，构建约束跟随误差，作为控制器设计的控制对象；
8.步骤4)，基于步骤1)建立的无人车动力学模型，确定综合描述系统不确定性界值的函数，结合步骤2)中得到的约束矩阵、约束向量，以及步骤3)构建的约束跟踪误差，构建自适应律与自适应鲁棒控制器；
9.进一步的，基于拉格朗日建模方法，建立侦察车和救援车统一的无人车动力学模型，具体为：
[0010][0011]
其中是不确定性，t表示时间，q(t)∈rn表示坐标，其为时间的函数，分别为速度与加速度，m(q,σ,t)是惯性矩阵，是科里奥利离心力，g(q,σ,t)是重力，是摩擦力及其他外部的干扰，τ∈rn是控制输入力矩，为了简化书写，后文中使用可分别代替表示
[0012]
针对系统的不确定性处理问题，对该模型进行分解，把动力学模型分解成标称部分和不确定部分：
[0013][0014][0015][0016][0017]
其中是标称部分，δm(
·
)，δc(
·
)，δg(
·
)，δf(
·
)为不确定部分。
[0018]
进一步的，步骤2)中，根据控制目标，建立侦察车所需要的目标到达约束，救援车所需要的轨迹跟踪约束和避障约束的数学模型，并以一阶约束和二阶约束的形式表达，具体为：
[0019]
(1)侦察车的到达约束：
[0020]
定义到达约束的数学模型：
[0021]
e1(t):＝h2(q(t))-s2，
[0022]
其中：
[0023][0024]
e1(t)为到达约束的数学模型，q(t)为侦察车坐标，为到达目标参考点坐标，s为目标到达区域的半径；
[0025]
求导，得到到达一阶伺服约束的数学模型：
[0026][0027]
其中l是一个常数且l＞0；
[0028]
求二阶导得到到达二阶伺服约束的数学模型：
[0029][0030]
从而得到到达约束矩阵：
[0031][0032]
到达约束向量：
[0033]
c1(q(t))＝ls
2-lh(q(t))，
[0034][0035]
(2)救援车的轨迹跟踪约束：
[0036]
定义轨迹跟踪约束的数学模型：
[0037]
e2(t)＝q2(t)-q1(t)，
[0038]
其中e2(t)为轨迹跟踪约束的数学模型，q1(t)为侦察车轨迹的坐标，q2(t)为救援
车的坐标；
[0039]
求导，得到轨迹跟踪一阶约束的数学模型：
[0040][0041]
求二阶导数得到轨迹跟踪二阶约束的数学模型：
[0042][0043]
从而得到轨迹跟踪约束矩阵：
[0044]
a2(q(t))＝[1 1]；
[0045]
轨迹跟踪约束向量：
[0046][0047][0048]
(3)救援车的避障约束：
[0049]
定义避障约束的数学模型：
[0050]
e3(t)＝ln(k-f)，
[0051]
其中：
[0052]
f＝||q(t)-q0||，
[0053]
e3(t)为避障约束的数学模型，q(t)为救援车的坐标，q0是障碍物坐标，k是安全范围的半径；
[0054]
求导，得到避障一阶约束的数学模型：
[0055][0056]
求二阶导数得到避障二阶约束的数学模型：
[0057][0058]
从而得到避障约束矩阵：
[0059][0060]
避障约束向量：
[0061]
c3(q(t))＝0，
[0062][0063]
进一步的，步骤3)，基于约束跟随理论对步骤2)建立的到达约束、轨迹约束和避障约束的数学模型进行分析，构建约束跟随误差，作为控制器设计的控制对象，具体方法为：
[0064]
构建到达约束的约束跟随误差：
[0065][0066]
构建轨迹跟踪约束的约束跟随误差：
[0067][0068]
构建避障约束的约束跟随误差：
[0069][0070]
包含到达约束的跟随误差β1、轨迹跟踪的约束跟随误差β2和避障约束的跟随误差β3的约束跟随误差向量为：
[0071]
β＝[β1,β2,β3]
t
。
[0072]
进一步的，步骤4)中，基于步骤1)建立的无人车动力学模型，确定综合描述系统不确定性界值的函数，结合步骤2)中得到的约束矩阵、约束向量，以及步骤3)构建的约束跟踪误差，构建自适应律与自适应鲁棒控制器，具体方法为：
[0073]
基于步骤1)的无人车动力学模型，分析其不确定性，确定不确定性参数σ的一般形式，并通过以下不等式进行放缩变换，得到综合描述系统不确定性界值的函数π(
·
)：
[0074][0075]
其中
[0076][0077]
其中α是函数π(
·
)中的不确定性变量，κ是控制增益，p是转移矩阵，ρe＞-1为常数，a是综合步骤2)中的约束矩阵，表示为a＝[a1(q)，a2(q)，a3(q)]
t
，c是综合两种约束的约束向量，表示为c＝[c1(q)，c2(q),c3(q)]
t
；；(
·
)-1
表示矩阵的逆，(
·
)
t
表示矩阵的转置；
[0078]
基于步骤2)构建的综合三种约束的约束跟随误差β，以及本步骤所构建的函数π(
·
)，构建能够自评估不确定性变量α的自适应律：
[0079][0080]
其中是对α的估计值，k1，k2＞0为设计参数。
[0081]
设计如下自适应鲁棒鲁棒控制器：
[0082][0083]
其中已在本步骤中写出，为下式
[0084][0085]
其中
[0086]
[0087][0088]
其中ξ＞0为常数。
[0089]
本发明还提供一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现所述的方法进行双无人车动态轨迹跟踪与避障自适应鲁棒控制。
[0090]
本发明与现有技术方案相比，其显著特点为：1)提出双无人车任务执行概念，由侦察车负责探测，由救援车进行动态轨迹跟踪与避障；2)将侦察车的目标到达、救援车的轨迹跟踪与避障控制目标转化成对于目标到达约束、轨迹跟踪约束与避障约束的满足，以约束跟随误差为所设计控制器的控制对象；3)将无人车动力学模型分成两部分考虑，能够应对无人车在工作中所面临的诸多不确定性因素，并由此设计对应的自适应鲁棒控制器，增强无人车抵抗干扰的能力，并具有优秀的控制性能。
附图说明
[0091]
图1为本发明双无人车动态轨迹跟踪与避障自适应鲁棒控制方法的侦察车到达约束的约束跟随误差效果图。
[0092]
图2为本发明双无人车动态轨迹跟踪与避障自适应鲁棒控制方法的救援车车轨迹跟踪约束的约束跟随误差效果图。
[0093]
图3为本发明双无人车动态轨迹跟踪与避障自适应鲁棒控制方法的救援车避障约束的约束跟随误差效果图。
[0094]
图4为本发明双无人车动态轨迹跟踪与避障自适应鲁棒控制方法的坐标误差效果图。
[0095]
图5为本发明双无人车动态轨迹跟踪与避障自适应鲁棒控制方法的侦察车与救援车运动轨迹效果图。
具体实施方式
[0096]
为了使本技术的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本技术进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本技术，并不用于限定本技术。
[0097]
本发明提出一种双无人车动态轨迹跟踪与避障自适应鲁棒控制方法，包括以下步骤：
[0098]
步骤1)、基于拉格朗日建模方法，建立侦察车和救援车统一的无人车动力学模型：
[0099][0100]
其中是不确定性，t表示时间，q(t)∈rn表示坐标，其为时间的函数，分别为速度与加速度，m(q，σ，t)是惯性矩阵，是科里奥利离心力，g(q，
σ，t)是重力，是摩擦力及其他外部的干扰，τ∈rn是控制输入，为了简化书写，后文中使用代替
[0101]
针对系统的不确定性处理问题，对该模型进行分解，把动力学模型分解成标称部分和不确定部分：
[0102][0103][0104][0105][0106]
其中是标称部分，δm(
·
)，δd(
·
)，δg(
·
)，δf(
·
)为不确定部分。
[0107]
为了方便后续控制器的设计，做如下定义：
[0108][0109][0110]
其中(
·
)-1
表示逆矩阵。
[0111]
步骤2)、根据控制目标，建立侦察车所需要的目标到达约束，救援车所需要的轨迹跟踪约束和避障约束的数学模型，并以一阶约束和二阶约束的形式表达；
[0112]
基于步骤1)建立的无人车动力学模型，分析其所需要完成的运动目标，包括侦察车到达指定位置以及救援车进行的轨迹跟踪和途中的避障行为，将运动目标进行数学化。
[0113]
首先定义侦察车到达约束的数学模型：
[0114]
e1(t):＝h2(q(t))-s2，
[0115]
其中：
[0116][0117]
e1(t)为侦察车到达约束的数学模型，q(t)为侦察车坐标，为到达目标参考点坐标，s为目标到达区域的半径；
[0118]
求导，得到到达一阶约束的数学模型：
[0119][0120]
其中l是一个常数且l＞0；
[0121]
求二阶导得到到达二阶约束的数学模型：
[0122][0123]
从而得到到达约束矩阵：
[0124][0125]
到达约束向量：
[0126]
c1(q(t))＝ls
2-lh(q(t))，
[0127][0128]
接下来，定义救援车轨迹跟踪约束的数学模型：
[0129]
e2(t)＝q2(t)-q1(t)，
[0130]
其中e2(t)为轨迹跟踪约束的数学模型，q1(t)为侦察车轨迹的坐标，q2(t)为救援车的坐标；
[0131]
求导，得到轨迹跟踪一阶约束的数学模型：
[0132][0133]
求二阶导数得到轨迹跟踪二阶约束的数学模型：
[0134][0135]
从而得到轨迹跟踪约束矩阵：
[0136]
a2(q(t))＝[1 1]；
[0137]
轨迹跟踪约束向量：
[0138][0139][0140]
再定义救援车避障约束的数学模型：
[0141]
e3(t)＝ln(k-f)；
[0142]
其中：
[0143]
f＝||q(t)-q0||，
[0144]
e3(t)为避障约束的数学模型，q(t)为救援车的坐标，q0是障碍物坐标，k是安全范围的半径；
[0145]
求导，得到避障一阶伺服约束的数学模型：
[0146][0147]
求二阶导数得到避障二阶伺服约束的数学模型：
[0148][0149]
从而得到避障约束矩阵：
[0150][0151]
避障约束向量：
[0152]
c3(q(t))＝0，
[0153][0154]
步骤3、基于约束跟随理论对步骤2)建立的到达约束、轨迹约束和避障约束的数学模型进行分析，构建约束跟随误差，作为控制器设计的控制对象。
[0155]
定义约束跟随误差：
[0156][0157]
从而得到到达约束的约束跟随误差：
[0158][0159]
轨迹跟踪约束的约束跟随误差：
[0160][0161]
避障约束的约束跟随误差：
[0162][0163]
包含到达约束的跟随误差β1、轨迹跟踪的约束跟随误差β2和避障约束的跟随误差β3的约束跟随误差向量为：
[0164]
β＝[β1,β2,β3]
t
。
[0165]
步骤4、基于步骤1)所建立的无人车动力学模型，分析其不确定性(模型不确定性、外界干扰等)，确定不确定性参数σ的一般形式，并通过以下不等式进行放缩变换，得到可以综合描述系统不确定性界值的函数π(
·
)：
[0166][0167]
其中:
[0168][0169]
其中α是函数π(
·
)中的不确定性变量，κ是控制增益的数值，p是转移矩阵，ρe＞-1为常数，a是综合步骤2中得到的a1(q)，a2(q)和a3(q)的约束矩阵，表示为a＝[a1(q)，a2(q)，a3(q)]
t
，c是综合步骤2中得到的c1(q)，c2(q)和c3(q)的约束向量，表示为c＝[c1(q)，c2(q)，c3(q)]
t
；为方便控制器设计，令；为方便控制器设计，令(
·
)-1
表示矩阵的逆，(
·
)
t
表示矩阵的转置；
[0170]
基于步骤2)构建的综合三种约束的约束跟随误差β，以及本步骤所构建的函数π(
·
)，构建能够自评估不确定性变量α的自适应律：
[0171][0172]
其中是对α的估计值，k1,k2＞0为设计参数。
[0173]
设计如下自适应鲁棒鲁棒控制器：
[0174]
[0175]
其中已在本步骤中写出，为下式
[0176][0177]
其中
[0178][0179][0180]
其中ξ＞0为常数。
[0181]
本发明还提出一种双无人车动态轨迹跟踪与避障自适应鲁棒控制系统，基于所述的方法进行双无人车动态轨迹跟踪与避障自适应鲁棒控制。
[0182]
一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现所述的方法进行双无人车动态轨迹跟踪与避障自适应鲁棒控制。该设备的硬件结构可以包括：处理器、存储器、输入/输出接口、通信接口和总线。其中处理器、存储器、输入/输出接口和通信接口通过总线实现彼此之间在设备内部的通信连接。处理器可以采用通用的cpu、微处理器、应用专用集成电路、或者一个或多个集成电路等方式实现，用于执行相关程序，以实现本说明书实施例所提供的技术方案。
[0183]
存储器可以采用rom、ram、静态存储设备，动态存储设备等形式实现。存储器可以存储操作系统和其他应用程序，在通过软件或者固件来实现本说明书实施例所提供的技术方案时，相关的程序代码保存在存储器中，并由处理器来调用执行。
[0184]
输入/输出接口用于连接输入/输出模块，以实现信息输入及输出。输入输出/模块可以作为组件配置在设备中，也可以外接于设备以提供相应功能。其中输入设备可以包括键盘、鼠标、触摸屏、麦克风、各类传感器等，输出设备可以包括显示器、扬声器、振动器、指示灯等。
[0185]
通信接口用于连接通信模块，以实现本设备与其他设备的通信交互。其中通信模块可以通过有线方式(例如usb、网线等)实现通信，也可以通过无线方式(例如移动网络、wifi、蓝牙等)实现通信。
[0186]
总线包括一通路，在设备的各个组件(例如处理器、存储器、输入/输出接口和通信接口)之间传输信息。
[0187]
实施例
[0188]
为验证本发明方案的有效性，对如下双无人车动态轨迹跟踪与避障问题进行仿真验证。控制对象和目标分别为：
[0189]
(1)控制对象为侦察车与救援车，侦察车与救援车统一的无人车运动方程为：
[0190]
[0191]
其中x1,y1和x2,y2分别为侦察车和救援车各自起始的坐标，m1是侦察车的质量，m2为救援车的质量，q
1x
,q
1y
和q
2x
,q
2y
分别为侦察车和救援车的输入控制，f
1x
,f
1y
,f
2x
,f
2y
是系统收到的外部干扰(包括地面阻力等等)。
[0192]
该系统写成步骤1中建立的动力学模型有：q＝[x1,y1,x2,y2]
t
，τ＝[q
1x
,q
1y
,q
1y
,q
2y
]，m＝diag(m1,m2,m3,m4)，d＝0,g＝0，f＝[f
x
,fy]
t
[0193]
(2)控制目标：由侦察车首先到达目标点并创建轨迹，由救援车实现轨迹跟踪与避障行为。
[0194]
各详细控制参数与数据如下：
[0195]
设置侦察车的初始位置为x1(0)＝1，y1(0)＝3，到达位置是以为参考点，半径为s＝0.7的圆，救援车的初始位置为x2(0)＝1，y2(0)＝3，避障点选取避障半径设置为r＝0.5m，外界干扰定义为δf＝[0.01sin(10t),0.01sin(10t),0.01sin(10t),0.01sin(10t)]
t
。同时在仿真过程中取侦察车与救援车的质量为m
1,2
＝1kg，＝1kg，控制器的设计参数为l＝0.1，k1＝1，k2＝0.3，κ＝50，ξ＝0.1，
[0196]
采用matlab进行仿真，仿真结果如图1，2，3，4，5所示。图1、2、3分别表示应用该控制方法的约束跟随误差β1,β2,β3，可以看出，侦察车到达约束的约束跟随误差在很短的时间，且系统稳定后跟踪误差波动范围极小，而轨迹跟踪约束跟随误差和避障约束跟随误差在障碍点附近发生波动，随后回到稳定状态，符合实际，具备较高的控制精度。图4中两条曲线分别表示侦察车与救援车运动轨迹在x和y方向上的差距，可以看出在避障区域附近，差距变大，完成避障后又回到稳定值0附近，符合实际，具有较高的控制精度。图5为无人作战平台轨迹图，可以看出，侦察车完成了目标到达后，救援车跟随前车轨迹，并对路径中出现的障碍物进行了躲避，完成避障后继续回到预定轨迹，具有较好的控制性能。因此本发明能够很好的解决双无人车动态轨迹跟踪与避障自适应鲁棒控制问题。
[0197]
以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
[0198]
以上所述实施例仅表达了本技术的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本技术构思的前提下，还可以做出若干变形和改进，这些都属于本技术的保护范围。因此，本技术专利的保护范围应以所附权利要求为准。