基于位姿空间误差度量的空间智能动作恢复策略生成方法与流程
本发明属于空间智能领域,具体涉及机器人执行控制技术,具体是基于位姿空间误差度量的空间智能动作恢复策略生成方法。
背景技术:
1、当前研究已在异常检测与恢复、鲁棒执行策略及扰动应对决策等方面取得进展。现有方法通常在检测到设备执行任务过程中遭遇意外扰动(物品跌落、意外碰撞、外力干扰)导致设备位姿偏离预期后,依据预设的恢复策略模板或简单重规划生成恢复动作,引导设备回归正常任务执行轨道。
2、但随着该类方法应用于扰动类型多样、偏离程度差异较大的实际执行场景,现有方法仍存在明显不足。
3、1、现有方法未将扰动导致的状态偏离纳入统一的位姿空间误差度量,仅依据经验阈值或单一维度的距离比较笼统判断偏离“轻微”或“严重”,难以准确区分该扰动应采用局部轨迹微调还是需要完全重新规划任务执行路径,容易对轻微扰动过度处理或对严重扰动处理不足。
4、2、现有方法对恢复策略候选方案执行后预测位姿与原任务后续路径下一路点位姿之间的空间衔接代价缺乏量化评估,某些恢复策略虽然能够使设备回归某一几何上可行的位姿,但该位姿与原任务后续路点在位置、姿态乃至过渡时间开销上的衔接代价较高,导致恢复后仍需额外的过渡调整才能继续原任务,现有方法未对这种衔接代价进行系统计算与比较。
5、3、现有方法对恢复执行过程本身的可回溯程度缺乏关注,一旦恢复策略执行后发现效果不佳,通常难以回退到扰动发生前或恢复执行中间某一具体位姿重新决策,也缺乏按位姿变化幅度采样关键可回溯节点的处理机制,容易在恢复失败后陷入更复杂的被动局面。
6、为此,提出基于位姿空间误差度量的空间智能动作恢复策略生成方法。
技术实现思路
1、针对现有技术存在的不足,本发明目的是提供基于位姿空间误差度量的空间智能动作恢复策略生成方法,本发明旨在保障对位姿偏差的准确恢复。
2、为了实现上述目的,本发明采用了如下技术方案:基于位姿空间误差度量的空间智能动作恢复策略生成方法,各步骤具体工作过程如下:
3、步骤s1:采集扰动发生后设备的实际三维位姿观测数据,获取设备的原始任务信息,对预期三维位姿进行采集,结合实际三维位姿观测数据,对设备的偏移状况进行评估,生成初步状态差异记录;
4、步骤s2:获取设备的精度要求,结合初步状态差异记录,对复合偏离度进行计算,并对设备进行偏离分级,基于偏离分级对设备进行恢复控制;获取恢复策略库,对设备的恢复候选方案进行提取,预测恢复候选方案执行后的设备位姿,结合设备的原始任务信息,对衔接代价进行计算,由衔接代价对恢复候选方案进行评分,并对恢复候选方案进行排序统计;
5、步骤s3:根据偏离分级与恢复候选方案评分,对定向局部恢复与独立恢复效果回溯核验进行触发判定,对设备的恢复候选方案进行选取与执行分流;
6、步骤s4:对执行定向局部恢复的设备进行获取,采集设备的原始任务信息,定位受扰动影响的局部位姿状态,对受扰动影响的局部位姿状态进行重新规划;对执行独立恢复效果回溯核验的设备进行获取,对设备进行预恢复,并对复合偏离度进行重新计算,重新对设备偏离状况进行核验;
7、步骤s5:对评估记录及核验结论进行统计,对恢复策略库进行填充优化。
8、进一步的,所述步骤s1的具体步骤如下:
9、步骤s11:当设备发生扰动,对扰动发生后的位置分量进行采集,得到实际位置分量p_actual,对扰动发生后的姿态分量进行采集,得到实际姿态分量r_actual;由实际位置分量与实际姿态分量,构成设备的实际三维位姿观测数据t_actual,t_actual=(p_actual,r_actual);
10、步骤s12:基于设备的原始任务信息,对设备的预期三维位姿进行采集,得到三维位姿预期数据,由三维位姿预期数据与实际三维位姿观测数据,对设备的偏移状况进行评估,对设备的偏差分量进行计算,由设备的偏差分量,生成初步状态差异记录。
11、进一步的,所述步骤s12的具体步骤如下:
12、步骤s121:基于设备的三维位姿预期数据,获取设备的预期位置分量p_expected与预期姿态分量r_expected;基于实际三维位姿观测数据,获取设备的实际位置分量p_actual与实际姿态分量r_actual;
13、步骤s122:由设备的预期位置分量p_expected与实际位置分量p_actual,对设备的位置偏移状况进行计算,得到设备的平移偏差分量e_pos;
14、由设备的预期姿态分量r_expected与实际姿态分量r_actual,对设备的姿态偏移状况进行计算,得到设备的旋转偏差分量e_rot;
15、将设备的平移偏差分量e_pos与旋转偏差分量e_rot进行统计,得到初步状态差异记录。
16、进一步的,所述步骤s2的具体步骤如下:
17、步骤s21:由设备的精度要求,对设备的可接受误差与分级阈值进行获取,根据设备的可接受误差,结合初步状态差异记录,将设备的平移偏差分量与旋转偏差分量进行整合,得到设备的复合偏离度,由复合偏离度与分级阈值对设备的偏离状况进行评估,对设备进行偏离分级;
18、步骤s22:对设备的恢复候选方案进行获取,预测恢复候选方案执行后的设备位姿,得到修复预测位姿数据t_j,t_j=(p_j,r_j);由设备的原始任务信息,对任务正常执行的后续位姿进行采集,得到正常后续位姿数据t_next,t_next=(p_next,r_next);由修复预测位姿数据、正常后续位姿数据结合恢复候选方案执行状况,对衔接代价进行评估,根据衔接代价对不同恢复候选方案进行评分,对恢复候选方案进行排序统计。
19、进一步的,所述步骤s21的具体步骤如下:
20、步骤s211:基于设备的可接受误差,获取设备的可接受最大平移误差与可接受最大旋转误差;由可接受最大平移误差设立平移权重系数α,由可接受最大旋转误差设立旋转权重系数β;根据初步状态差异记录,对设备的平移偏差分量e_pos与旋转偏差分量e_rot进行提取,结合平移权重系数α与旋转权重系数β进行计算,得到复合偏离度d;
21、步骤s212:获取复合偏离度d与分级阈值τ_minor,将复合偏离度d与分级阈值τ_minor进行对比,若d<τ_minor,表明设备存在轻微偏离,需对设备进行定向局部恢复;若d≥τ_minor,表明设备存在严重偏离,需对设备进行重新规划。
22、进一步的,所述步骤s22的具体步骤如下:
23、步骤s221:基于修复预测位姿数据t_j,对修复位置分量与修复姿态分量进行采集,基于正常后续位姿数据t_next,对后续位置分量与后续姿态分量进行采集,由修复位置分量与后续位置分量进行计算,得到衔接位置偏差d_pos;
24、由修复姿态分量与后续姿态分量进行计算,得到设备的衔接旋转偏差d_rot;
25、获取设备的最大控制速度,结合衔接位置偏差与衔接旋转偏差,对恢复候选方案执行后额外的过渡时间开销进行计算,得到额外时间开销δt_j;
26、将衔接位置偏差、衔接旋转偏差与额外时间开销进行结合计算,得到衔接代价cost_j;
27、步骤s222:基于衔接代价cost_j,对恢复候选方案的衔接状况进行评估,得到衔接合理度评分score_j;
28、对恢复策略库所提供的恢复候选方案进行统计,对每个恢复候选方案的衔接合理度评分进行记录,根据恢复候选方案的衔接合理度评分,对恢复候选方案进行降序排序,基于排序结果对恢复候选方案进行统计。
29、进一步的,所述步骤s3的具体步骤如下:
30、步骤s31:基于偏离分级对设备的偏离状况进行采集,若设备存在轻微偏离,将设备标注为定向局部恢复,若设备存在严重偏离,将设备标注为独立恢复效果回溯核验;
31、步骤s32:根据恢复候选方案评分,对评分最高与次高的恢复候选方案进行采集,设立评分接近阈值,若评分最高与次高的恢复候选方案的评分差值低于评分接近阈值,将评分最高与次高的恢复候选方案进行记录,分别对设备进行独立恢复效果回溯核验;若低于评分接近阈值,对评分最高的恢复候选方案进行记录,结合设备的标注结果,对设备进行恢复控制。
32、进一步的,所述步骤s4的具体步骤如下:
33、步骤s41:获取定向局部恢复设备,采集设备对应的最优恢复候选方案,由设备的原始任务信息,对设备的实际三维位姿观测数据与正常后续位姿数据进行获取,根据最优恢复候选方案,将设备由实际三维位姿观测数据向正常后续位姿数据进行过渡,生成局部恢复动作序列,对设备执行局部恢复;
34、步骤s42:对完成局部恢复的设备与需执行独立恢复效果回溯核验的设备进行获取,采集设备记录的恢复候选方案,对设备进行预恢复,将预恢复结果与正常后续位姿数据进行结合计算,对设备偏离状况进行核验。
35、进一步的,所述步骤s41的具体步骤如下:
36、步骤s411:对受扰动影响的局部位姿状态进行定位,采集设备的实际三维位姿观测数据与正常后续位姿数据,结合最优恢复候选方案,对局部恢复动作序列进行获取,由局部恢复动作序列,对设备进行恢复,对设备恢复过程中的设备位姿变化进行记录;
37、步骤s412:对恢复过程中的位姿点进行采集,记录位姿点的位姿数据t_k,t_k=(p_k,r_k),对相邻位姿点之间的复合偏离度进行计算,对复合偏离度的均值进行获取,将高于均值的复合偏离度对应的位姿点进行记录,设立为可回溯节点。
38、进一步的,所述步骤s42的具体步骤如下:
39、步骤s421:对完成局部恢复的设备,将恢复完成结果与正常后续位姿数据之间的复合偏离度进行重新计算,由设备的精度要求,获取核验阈值;由核验阈值对复合偏离度进行校验,若核验通过,则进行执行任务;
40、步骤s422:对存在严重偏离的设备,进行重新规划任务路径,对存在多个恢复候选方案,将每个恢复候选方案进行独立执行,记录设备执行后的实际位姿,结合正常后续位姿数据,计算方案执行后的复合偏离度,结合核验阈值进行判断,若核验通过,对该方案进行记录,对设备进行执行。
41、综上所述,由于采用了上述技术方案,本发明的有益效果是:
42、1、本发明通过将设备实际位姿与预期位姿之间的平移偏差与姿态(旋转)偏差进行复合,对偏差状况进行判断,该方式提高了偏差判断的准确性,基于偏差状况对修复路径进行分流,保障修复处理与偏差状况相贴合,提高路径选择的准确性。
43、2、本发明通过将恢复策略候选方案执行后预测位姿与原任务后续路径下一路点位姿之间的空间衔接代价进行详细评估,通过位置偏差、位姿偏差与时间开销进行整体分析,保障对候选方案选择的准确性。
44、3、本发明通过局部重定向对恢复过程中的位姿点进程采样,记录恢复过程中的恢复效果,当出现效果不佳的状况,能够准确对原有方案进行回溯处理,确保整体恢复的稳定性与安全性。
版权声明:本站专利文献数据主要来源于国家知识产权局等公开渠道,由 技术新讯 进行整理汇编。页面设计及数据汇编的著作权归本站所有。全部内容仅供个人学习、科研评估与技术交流参考,严禁用于任何商业用途(包括但不限于直接生产制造、技术转让、倒卖数据等)。
免责条款:专利技术具有时效性,本站不对资料的绝对准确性、完整性和可实施性提供任何明示或暗示的担保。因参考本站内容造成的直接或间接损失,本站不承担相关法律责任。若您认为本站内容有侵权问题,请及时提供权属证明与我们联系,本站将在核实后依法处理。反馈意见:yyfuon@163.com