一种求解机会约束轨迹规划问题的数值积分辅助凸近似方法

1.本发明属于轨迹规划领域，涉及一种求解机会约束轨迹规划问题的数值积分辅助凸近似方法。


背景技术：

2.目前，最优控制方法由于能够在统一的框架下考虑装备运行过程中的各类约束条件，并可以满足系统的运动方程，成为了求解装备轨迹规划问题的重要数学工具。然而在装备实际使用过程中，作业环境与装备本身都存在大量的不确定性，传统的确定性最优控制框架无法考虑这些不确定性因素，因此生成的轨迹难以保证装备的作业效能。机会约束最优控制为描述不确定性下装备轨迹规划提供了有力的数学工具，常采用凸近似方法进行求解。
3.传统凸近似方法中，使用采样完成对指示函数的凸近似函数的期望值的近似。为实现高精度的近似，通常采用数千个乃至上万个采样点的大规模采样过程。然而现有的计算最优控制方法通常采用迭代求解的框架，这意味着使用传统凸近似方法时，每个迭代步中每个机会约束被评估的次数达数千次之多，成为了影响求解效率的主要因素。当问题中装备的运动方程或约束条件具有高度非线性时，在非线性与大规模采样带来的双重计算负担下，难以在能容忍的时间内完成问题求解。
4.因此，必须对传统凸近似方法进行改进，以解决其求解强非线性机会约束最优控制时的效率瓶颈。


技术实现要素：

5.为了解决上述技术问题，面向不确定性变量概率分布已知的情况，本发明提出了一种求解机会约束轨迹规划问题的数值积分辅助凸近似方法。在所提出的方法中，使用数值积分过程代替传统凸近似方法中的大规模采样过程，每个机会约束尽在采用的积分格式中所对应的积分点上进行评估，而通常采用100个以内积分点就足以实现高精度的数值积分。因此，所提出的方法能够显著提升机会约束轨迹规划问题的求解效率。
6.为了达到上述目的，本发明采用的技术方案为：
7.一种求解机会约束轨迹规划问题的数值积分辅助凸近似方法，首先，根据装备的运动学方程、约束条件、边界条件，建立面向轨迹规划的确定性最优控制问题；第二，厘清作业过程中不确定性的来源，设定允许的违约率，在确定性最优控制问题的基础上，建立机会约束最优控制问题；第三，基于指示函数及其近似函数对存在的机会约束进行转化，使用数值积分计算指示函数的近似函数的期望值，实现机会约束向确定性约束的转化；最后，使用常规的计算最优控制方法对转化形成的确定性最优控制问题进行求解。包括以下步骤：
8.步骤1：根据装备的运动学方程、约束条件、边界条件，建立面向轨迹规划的确定性最优控制问题：
[0009][0010]
其中，u表示控制输入；x表示被控系统的状态空间；t∈[t0,tf]表示时间域，其中t0表示初始时刻，tf表示末端时刻；j(
·
)表示目标函数，其中右端第一项称为末端性能指标，体现了对末端的要求，第二项为积分性能指标，体现了对系统状态变化过程中的状态x(t)与u(t)的要求；表示被控系统的动力学方程；g(
·
)表示不等式形式的过程约束，；b(
·
)表示初始/终端约束；
[0011]
式所形成的最优控制问题是在时间域t∈[t0,tf]内极小化目标函数j(
·
)得到状态量x(t)以及控制输入u(t)。对于轨迹规划问题，通常将目标函数j(
·
)设置成能量与时间的组成。其中动力学方程f(
·
)由装备的运动学方程推导而来；过程约束：g(x,u,t)≤0表示为避障约束；初始/终端约束：b(x(t0),t0,x(tf),tf)＝0根据约束/边界条件与任务需求建立；
[0012]
步骤2：厘清作业过程中不确定性的来源，设定允许的违约率，在确定性最优控制问题的基础上，建立机会约束最优控制问题：
[0013][0014]
其中，pr(
·
)表示求概率；g(x,u,t；ξ)表示引入随机变量后的不确定性避障约束；ξ表示与约束不确定相关的随机变量；ε∈(0,1]是预先给定的风险参数，也称为约束违反率。
[0015]
不确定性来源的考虑体现在名为机会约束的数学列式当中：pr(g(x(t),u(t),p,t；ξ)＞0)≤ε，其意义为允许约束条件能以一定的概率值满足。
[0016]
由上述公式2，不确定性轨迹规划问题被转化为机会约束最优控制问题，然而该问题中的机会约束是以概率的形式存在，不能直接被数值求解，因此需要将其转化为能被数值求解的确定性约束，而某事件发生的概率可被描述为指示函数的期望，因此可以通过对该期望的求解来转化机会约束。
[0017]
步骤3：基于指示函数及其近似函数对存在的机会约束进行转化，使用数值积分计算指示函数的近似函数的期望值，实现机会约束向确定性约束的转化。
[0018]
由于机会约束项pr(g(ξ)＞0)是以概率形式存在，因此需要对其进行转化。对于概率空间(ω,γ,p)的事件f，我们有pr(f)＝e[w(f)]，其中w是指示函数，定义为：
[0019][0020]
因此机会约束可以写成：
[0021][0022]
每个机会约束中考虑单个不确定源时，公式可写成如下积分形式：
[0023][0024]
其中r为对应不确定源的概率分布函数。不确定源的概率分布函数决定了积分的上下界。如当ξ满足区间(m,n)上的均匀分布时，有积分上界ub＝m和积分下界lb＝n；当ξ满足均值为σ，标准差为σ的正态分布时，有积分上界ub＝μ-kσ和积分下界lb＝μ kσ。其中选用的k值应至少超过3。
[0025]
通过使用lgl型积分方案，式可以被重新表达为：
[0026][0027]
其中为lgl积分点，为对应的权重系数，ni为使用的积分点的数目。
[0028]
由于指示函数w的不连续性，通常采用近似函数来代替，如下三种所示：
[0029]
ψ1(p)＝exp(αp)
ꢀꢀ
(7)
[0030][0031][0032]
至此机会约束完成了向确定性约束的转化。
[0033]
步骤4：使用常规的计算最优控制方法对转化形成的确定性最优控制问题进行求解
[0034]
形成的确定性最优控制问题由matlab软件包iclocs构建，进而使用ipopt求解器解决所产生的非线性规划问题。
[0035]
本发明的有益效果为：
[0036]
相比于传统凸近似方法使用了大量的采样，该发明使用了高精度的数值积分来计算指示函数的期望值，进而对机会约束进行处理。从结果上来看，数值积分辅助凸近似方法得到的违约率与传统方法得到的违约率处于类似水平，并且其计算效率比传统方法高出10倍以上。因此可以得出，数值积分辅助凸近似方法具有有效性与求解效率高的特点，可以为求解机会约束轨迹规划问题提供了新思路。
附图说明
[0037]
图1为本发明的流程图。
[0038]
图2为本发明实施例中两轮差速小车的模型图；图中，表示方向角，r表示车轮半径，b表示双轮间距，l表示小车特征圆的中心点和两轮中点间的距离，rc表示特征圆的半径，c表示特征圆的中心点，om表示两轮中点。
[0039]
图3为本发明实施例中障碍物示图；图中，a
obs,i
表示准矩形的长半径，b
obs,i
表示准
矩形的宽半径，c
obs,i
表示准矩形的中心，θ
obs,i
表示其朝向。
[0040]
图4为本发明实施例中灯光躲避示意图；图中，c
wt,i
表示瞭望塔的中心点，θ
wt,i
表示初始时刻方位角，ω
wt,i
表示灯塔灯光的转速，a
wt,i
表示灯光的长轴，b
wt,i
表示灯光的短轴。
[0041]
图5为本发明实施例中两种方法计算得到的结果图；图5(a)为小车方向角随时间演化图，图5(b)为小车线速度随时间演化图，图5(c)为小车角速度随时间演化图，图5(d)为违约率随时间演化图。
[0042]
图6为本发明实施例中使用辅助数值积分凸近似法得到的最优轨迹；图6(a)为规划的小车轨迹图，图6(b)为在高违约率时刻的局部放大图。
具体实施方式
[0043]
以下结合具体实施例对本发明做进一步说明。
[0044]
考虑两轮差速驱动小车作为轨迹规划的对象如图2所示。小车的相关参数如表1所示。考虑小车的横坐标x，纵坐标y，方向角线速度v和角速度ω组成的状态空间并考虑车轮左扭矩τr和右扭矩τ
l
为控制输入u＝[u1,u2]
t
＝[τr,τ
l
]
t
。考虑地图中同时存在静态障碍与灯塔，静态障碍被视为准矩形或是圆如图3所示，灯塔的灯光被视为动态的椭圆如图4所示。可根据不同的数学列式构建避障关系的不等式约束：
[0045][0046]
其中c
obs,i
(x
obs,i
,y
obs,i
)表示准矩形的中心点。a
obs,i
和b
obs,i
是准矩形的长度和宽度，p
obs,i
值越高越接近矩形。
[0047][0048]
其中l
wt,i
是c
wt,i
点和相应灯光中心之间的距离；α
wt,i
＝θ
wt，i
ω
wt,i
(t-ts)表示灯光转过的角度，其中θ
wt,i
表示灯光的初始角度，ω
wt,i
表示灯光的转速。障碍参数与灯塔参数设置如表2和表3所示。
[0049]
设定小车的初始边界条件为x
ini
＝[0,0,0,0,0]
t
，对应的位置(0m,0m)，方向为0度。终端边界条件为x
ter
＝[20,20,90,0,0]
t
，对应的位置(20m,20m)，方向是90度。
[0050]
表1小车参数设置
[0051][0052]
表2障碍参数设置
[0053][0054]
表3灯光参数设置
[0055][0056]
一种求解机会约束轨迹规划问题的数值积分辅助凸近似方法，包括以下步骤：
[0057]
步骤1：根据装备的运动学方程、约束条件、边界条件，建立面向轨迹规划的确定性最优控制问题
[0058]
根据小车的运动学方程推导出动力学方程如下：
[0059][0060]
约束条件即避障条件，如式和。起始与终端条件分别为：
[0061][0062]
目标函数j(
·
)设置成能量与时间的组成，r为单位矩阵。最终建立确定性最优控制问题：
[0063][0064]
步骤2：厘清作业过程中不确定性的来源，设定允许的违约率，在确定性最优控制问题的基础上，建立机会约束最优控制问题
[0065]
考虑灯光转速ω
wt
为不确定源，并满足均匀分布ξ～u(55,65)，预先设定ε＝0.1的约束违反率。去除问题中的确定性约束，并引入机会约束的表达：
[0066]
pr(h
wt
(x,u,ξ,t)＞0)≤ε
ꢀꢀ
(15)
[0067]
其中h
wt
(x,u,ξ,t)＞0为式中确定的灯光转速被替换成不确定源ξ后的列式：
[0068][0069]
其中α
wt,i
＝θ
wt
ξ(t-ts)。因此，新建立的机会约束最优控制问题如下：
[0070][0071]
其中公式中所有参数的设置如表1-3所示。
[0072]
步骤3：基于指示函数及其近似函数对存在的机会约束进行转化，使用数值积分计算指示函数的近似函数的期望值，实现机会约束向确定性约束的转化
[0073]
由于机会约束项pr(g(ξ)＞0)是以概率形式存在，因此需要对其进行转化。对于概率空间(ω,γ,p)的事件f，我们有pr(f)＝e[w(f)]，其中w是指示函数，定义为：
[0074][0075]
因此机会约束可以写成：
[0076][0077]
每个机会约束中考虑单个不确定源时，公式可写成如下积分形式：
[0078][0079]
当ξ满足均匀分布如ξ～u(m,n)时，有ub＝55和lb＝65；
[0080]
通过使用lgl型积分方案，式可以被重新表达为：
[0081][0082]
其中为lgl积分点，为对应的权重系数，ni被设置为50。
[0083]
由于指示函数w的不连续性，采用近似函数ψ3来代替，并设定相关参数为c＝200，m1＝10和m2＝1。
[0084][0085]
至此机会约束完成了向确定性约束的转化。
[0086]
步骤4：使用常规的计算最优控制方法对转化形成的确定性最优控制问题进行求解
[0087]
形成的确定性最优控制问题由matlab软件包iclocs构建，进而使用ipopt求解器解决所产生的非线性规划问题。
[0088]
使用传统凸近似与数值积分辅助凸近似方法得到的计算结果报告如图5所示。图5(a)、图5(b)、图5(c)分别展示了小车朝向、速度和角速度的演化曲线。图5(d)给出了约束违约历史。可以看出，两种算法得到的结果都很相似，而且这两种方法的约束违反率都在规定的值以内。这验证了数值积分辅助凸近似法的有效性和准确性。在表4中，我们报告了两种方法的计算性能，可以发现而这产生的违约率较为接近，然而本发明提出的数值积分辅助凸近似方法具有更高的计算效率。
[0089]
表4两种方法的计算对比结果
[0090][0091]
图6(a)描述了由数值积分辅助凸近似法获得的小车的轨迹。由于我们在图5(d)中观察到7.62s的高约束违反率，此刻探照灯的配置被采样1000次，并绘制成深色椭圆。图6(b)显示了此刻的放大图。可以看出，小车在此刻拥有被探测的高风险。
[0092]
相比于传统凸近似方法使用的大量采样，本发明使用了高精度的数值积分来计算指示函数的期望值，进而对机会约束进行处理，实现了对机会约束轨迹规划问题的求解。从结果上来看，数值积分辅助凸近似方法得到的违约率与传统方法得到的违约率处于类似水
平，并且其计算效率远高于传统方法。数值积分辅助凸近似方法具有有效性与求解效率高的特点，为求解机会约束轨迹规划问题的高质量求解提供了新方法。
[0093]
以上所述实施例仅表达本发明的实施方式，但并不能因此而理解为对本发明专利的范围的限制，应当指出，对于本领域的技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些均属于本发明的保护范围。