一种轨迹优化方法及系统
本发明涉及轨迹规划与优化,具体涉及一种轨迹优化方法及系统。
背景技术:
1、变分法是求解两点边值轨迹优化问题的主流方案之一,广泛应用于机器人轨迹规划、自动驾驶路径规划和航天器交会等场景;该类方法通过对参考轨迹逐次线性化、求解线性修正方程并迭代更新轨迹,能够完整保留系统动力学结构,适配复杂非线性约束;然而,传统变分法存在两个核心瓶颈:其一,每轮迭代必须求解线性修正方程组,随着离散节点增多,矩阵维度快速膨胀、条件数恶化,单次线性求解耗时极高;其二,迭代步长难以平衡,大步长易导致轨迹震荡发散,小步长则收敛速度缓慢,常规方案通常需要上百次迭代才能达到合格轨迹精度,难以满足在线避障、故障后重规划等实时控制需求。
2、近年来,伊辛机、量子退火机等类量子优化硬件快速发展,其擅长并行求解组合优化问题;现有研究通常将离散后的整条轨迹、控制变量及相关约束统一编码为单个qubo/伊辛模型并交由伊辛机直接求解。此类方案存在明显缺陷:所需二进制变量数随离散节点数、决策变量维数和编码位数的增加而急剧增长,变量间的约束耦合进一步推高模型复杂度,在高维或高分辨率轨迹规划问题中面临严重的硬件规模与连接限制;此外,该方案未解决连续变量离散编码的精度与范围矛盾:编码区间过宽则离散间隔粗糙、解码精度不足,编码区间过窄则实际所需修正量超出搜索范围,伊辛机无法输出有效更新。
3、另有少量研究将伊辛求解与信赖域优化相结合用于数值优化,其通常基于hessian矩阵构建连续二次近似子问题,并以固定阈值单一尺度调节信赖域半径。该类方案存在明显不足:依赖hessian矩阵重构子问题导致计算开销大;仅调节信赖域半径单一尺度,未区分编码区间尺度与更新步长尺度,无法协同平衡离散编码精度与轨迹更新幅度;缺乏编码区间自适应调节机制,无法解决有限比特下连续变量离散化的精度与范围冲突;且未适配变分轨迹迭代的线性修正子问题场景。
技术实现思路
1、本发明的目的在于克服现有技术的不足,提供了一种轨迹优化方法及系统。
2、为实现上述目的,本发明提供如下技术方案:
3、本技术提供一种轨迹优化方法,包括:
4、获取被控对象的初始参考轨迹;
5、对所述初始参考轨迹进行线性化处理,得到线性修正子问题;
6、根据所述线性修正子问题的修正量基准尺度,确定编码区间,基于所述编码区间将所述线性修正子问题映射为组合优化模型,并利用优化求解器对所述组合优化模型进行求解,得到离散修正量;
7、根据信赖域半径对所述离散修正量进行投影约束,使投影后的修正量满足所述信赖域半径的约束;
8、根据所述投影后的修正量生成试探轨迹,并根据所述试探轨迹的残差判定是否接受所述试探轨迹作为下一轮迭代的参考轨迹;
9、根据所述离散修正量与所述信赖域半径的关系,调节所述编码区间的尺度和/或所述信赖域半径;
10、迭代执行上述步骤直至满足收敛条件,输出优化后的轨迹。
11、进一步地,所述线性修正子问题通过以下方式构建:
12、计算当前参考轨迹的动力学残差,对所述动力学残差在当前参考轨迹邻域内进行线性化,构建线性修正方程组,并将所述线性修正方程组转化为最小二乘优化问题;
13、所述动力学残差表示为:
14、
15、其中,为时间尺度参数,表示动力学方程给出的状态变化率,表示由当前离散轨迹计算得到的状态变化率;
16、所述线性修正方程组表示为:
17、
18、其中,为线性化后的系数矩阵,为待求解的修正量向量,为残差向量;
19、所述最小二乘优化问题表示为:
20、
21、所述最小二乘优化问题展开为连续二次型目标函数:
22、
23、其中,,。
24、进一步地,所述编码区间的确定方式包括:
25、第轮迭代第个连续变量的编码区间上下界分别确定为:
26、
27、其中,为变分演化第轮迭代第个变量的修正量基准值,为第轮迭代的编码尺度系数;
28、在固定编码比特数的条件下,第个连续变量的离散间隔为:
29、
30、偏移量为;
31、连续修正量表示为离散编码形式:
32、
33、其中,为二进制编码所对应的整数。
34、进一步地,所述组合优化模型为二次无约束二值优化模型或伊辛哈密顿模型;
35、将所述连续二次型目标函数代入所述离散编码形式,得到二次无约束二值优化模型;
36、利用自旋转换关系将所述二次无约束二值优化模型转化为伊辛哈密顿模型:
37、
38、其中,为取值为±1的伊辛自旋变量,为耦合矩阵,为偏置场。
39、进一步地,所述根据信赖域半径对所述离散修正量进行投影约束,包括:
40、计算步长利用率:
41、
42、其中,为所述离散修正量,为当前信赖域半径;
43、若>1,则将所述离散修正量的模长投影至所述信赖域半径:
44、
45、使投影后的修正量满足。
46、进一步地,所述根据所述试探轨迹的残差判定是否接受所述试探轨迹,包括:
47、计算当前轨迹残差和试探轨迹残差;
48、若,则接受所述试探轨迹作为下一轮迭代的参考轨迹;
49、若,则保留当前参考轨迹,并缩小所述信赖域半径。
50、进一步地,所述调节所述编码区间的尺度和/或所述信赖域半径,包括:
51、以所述步长利用率为统一反馈量,分别调节编码尺度系数和信赖域比例系数,其中所述信赖域半径;
52、所述编码尺度系数的调节规则为:
53、当时,;
54、当时,或保持不变;
55、当时,;
56、其中,为编码尺度系数的低阈值,为编码尺度系数的高阈值;
57、所述信赖域比例系数的调节规则为:
58、当时,;
59、当时,保持不变或小幅衰减;
60、当时,;
61、其中,为信赖域比例系数的低阈值,为信赖域比例系数的高阈值,且。
62、进一步地,所述收敛条件为:
63、相邻两轮迭代的相对残差下降率小于0.006时,判定残差趋于稳定,终止迭代。
64、进一步地,还包括轨迹精度评价步骤:
65、以高精度数值积分轨迹为基准,计算轨迹平均绝对误差:
66、
67、其中,为离散采样点数量,为轨迹状态维度,为第次迭代在时刻第维状态变量值,为通过数值积分得到的参考状态轨迹在时刻第维状态变量值。
68、一种轨迹优化系统,用于执行上述的方法,包括:
69、获取模块,用于获取被控对象的初始参考轨迹;
70、线性化模块,用于对所述初始参考轨迹进行线性化处理,构建线性修正子问题;
71、编码与映射模块,用于根据所述线性修正子问题的修正量基准尺度确定编码区间,基于所述编码区间将所述线性修正子问题映射为组合优化模型;
72、优化求解器,用于对所述组合优化模型进行求解,得到离散修正量;
73、投影约束模块,用于根据信赖域半径对所述离散修正量进行投影约束,使投影后的修正量满足所述信赖域半径的约束;
74、判定与更新模块,用于根据所述投影后的修正量生成试探轨迹,根据所述试探轨迹的残差判定是否接受所述试探轨迹作为下一轮迭代的参考轨迹;
75、参数调节模块,用于根据所述离散修正量与所述信赖域半径的关系,调节所述编码区间的尺度和/或所述信赖域半径;
76、收敛判定模块,用于判断是否满足收敛条件,并在满足时输出优化后的轨迹。
77、与现有技术相比,本技术具有以下有益效果:
78、本发明提出一种轨迹优化方法及系统,通过将每轮变分迭代产生的线性修正子问题映射为组合优化模型并利用伊辛机进行并行求解,而非对完整轨迹进行全局离散编码,有效降低了单轮建模和求解负担,减少了二进制变量数量与约束耦合规模,提升了硬件适配性;通过以步长利用率为统一反馈量分别调节编码区间尺度和信赖域半径的双尺度自适应机制,在迭代初期信赖域半径大、编码区间宽以快速将轨迹拉向可行域,在迭代后期自动收缩区间、缩小离散间隔以完成局部精细修正,同步兼顾了搜索范围与解码精度,解决了有限比特下连续变量离散化的精度与范围冲突;通过当修正量超出信赖域边界时保持方向不变并将模长投影至信赖域半径、当试探轨迹残差未有效下降时保留原参考轨迹并自动收缩信赖域的策略,有效避免修正量超出线性化有效区域和无效的大步长迭代,保证了轨迹演化的稳定性;实测数据表明,本发明在二维轨迹,三维高非线性算例中加快轨迹演化,迭代效率的大幅提升使得本发明能够适配飞行器故障重规划、机器人在线避障等低时延任务;此外,本发明无需构建hessian矩阵,计算开销低,可适配二维、三维及多种动力学模型的轨迹优化场景。
版权声明:本站专利文献数据主要来源于国家知识产权局等公开渠道,由 技术新讯 进行整理汇编。页面设计及数据汇编的著作权归本站所有。全部内容仅供个人学习、科研评估与技术交流参考,严禁用于任何商业用途(包括但不限于直接生产制造、技术转让、倒卖数据等)。
免责条款:专利技术具有时效性,本站不对资料的绝对准确性、完整性和可实施性提供任何明示或暗示的担保。因参考本站内容造成的直接或间接损失,本站不承担相关法律责任。若您认为本站内容有侵权问题,请及时提供权属证明与我们联系,本站将在核实后依法处理。反馈意见:yyfuon@163.com