基于趋近风险分级和动态制动余量的移动平台跟随控制方法与流程
本发明涉及移动机器人控制,具体涉及一种基于多重滤波与动态制动余量的移动平台跟随控制方法及系统,适用于智能小车、服务机器人、移动载具等需要精确跟随移动目标的应用场景。
背景技术:
1、在移动机器人跟随控制领域,尤其是基于uwb(超宽带)定位技术的跟随系统中,移动平台需要实时获取目标(如行人或其他移动设备)的距离和方位信息,并据此驱动自身实现自主跟随。uwb技术因其测距精度较高、抗多径能力相对较强等优势,已成为室内外短距离跟随应用的主流感知手段之一。然而,在实际工程应用中,uwb模块受多径效应、电磁干扰、遮挡以及信号衰减等因素影响,所采集的原始距离和角度数据往往包含显著的噪声和偶发跳变,如何从含噪数据中提取可靠的控制输入是该领域面临的首要问题。
2、针对uwb数据的噪声抑制问题,现有技术已提出多种滤波方案。例如采用低通滤波器对测距数据进行平滑处理,以抑制高频噪声;采用中值滤波对距离序列进行去噪处理。然而,单一滤波手段难以兼顾高频噪声抑制和突发跳变消除的双重需求:低通滤波虽能平滑噪声,但会引入相位滞后,导致跟随动作延迟;中值滤波虽能有效消除孤立跳变点,但对连续变化的噪声抑制能力有限。此外,现有方案普遍缺乏对数据有效性的前置判断和跳变确认机制,容易将无效帧或跳变帧直接引入控制环路,造成误动作。
3、在跟随控制的制动决策方面,现有系统通常采用固定距离阈值触发制动,或基于固定时间窗口进行速度预测。例如,部分方案设定当跟随距离小于某一固定值(如1.5m)时触发减速停止;另有方案采用固定预测时间(如2s)估算未来位置并提前制动。然而,上述方法未充分考虑平台当前速度、目标趋近速度、从感知到执行的全链路延迟以及当前可用减速度等动态因素,导致制动触发时机不当:制动过晚会造成急停甚至碰撞风险,制动过早则频繁减速、影响跟随效率。尤其是当目标快速靠近或平台自身速度较高时,固定阈值方案的风险尤为突出。
4、在速度切换与状态管理方面,现有系统多采用离散档位切换策略,即根据距离区间直接跳转至对应的速度档位,缺乏平滑过渡机制。这种生硬的速度跳变不仅造成机械冲击和乘坐舒适性下降,还增加了电机和传动系统的磨损。同时,现有跟随控制在跟踪、制动和停止状态之间通常采用单一距离阈值直接切换,缺少进入/退出滞回设计和连续确认条件,易在阈值附近频繁跳转,表现为速度振荡和反复进退。
5、此外,部分研究提出采用模型预测控制(model predictive control, mpc)等先进算法对目标运动进行预测并优化控制序列,理论上可实现更优的跟随轨迹。然而,mpc方法计算量大,对主控芯片算力要求高,且高度依赖精确的目标运动模型,在低成本嵌入式平台上难以实时部署,工程实用性受限。
6、综上,现有移动平台跟随控制技术在数据可靠性处理、制动时机自适应决策、速度平滑切换以及状态抗振控制等方面仍存在明显不足,亟需一种能够综合解决上述问题的跟随控制方案。
技术实现思路
1、为解决上述技术问题,本发明的采用的一个技术方案如下:
2、基于趋近风险分级和动态制动余量的移动平台跟随控制方法,包括以下步骤:
3、s1):数据获取与预处理:以预设周期获取跟随目标与移动平台之间的实时距离数据和角度数据,对所述实时距离数据和角度数据进行数据有效性判断、跳变检测与确认、以及加权滤波处理,得到滤波后的距离值、滤波后的角度值以及距离变化趋势。
4、其中,所述数据有效性判断包括对数据帧进行帧格式校验、字段有效性检查和信号质量评估,剔除格式错误、信号质量不满足预设条件或数据值明显越界的无效帧,仅对判定为有效的帧进行后续处理。
5、所述跳变检测与确认包括:将当前有效帧的数据值与上一有效帧的数据值进行比较,当距离变化量超过预设的距离跳变阈值或角度变化量超过预设的角度跳变阈值时,将该帧标记为候选跳变帧,不立即采纳其数据,而是启动确认计数器;仅当连续预定数量的后续帧均确认跳变方向与所述候选跳变帧一致时,才接受该新数据值,否则保持上一有效数据值。
6、所述加权滤波处理包括:对经过跳变检测确认后的距离数据,根据目标远离或靠近的不同趋势采用不同的平滑策略;对经过跳变检测确认后的角度数据,根据角度变化量的大小采用不同的平滑策略,分别输出滤波后的距离值和角度值。同时,基于连续两帧滤波后的距离值计算距离变化量,并结合数据更新周期确定目标相对于平台的距离变化趋势。
7、s2):趋近速度计算与动态制动需求距离估算:根据步骤s1输出的连续两帧滤波后的距离值以及所述预设周期,计算目标相对于平台的趋近速度,所述趋近速度用于表征目标与平台之间的距离缩短风险。
8、根据传感器采样延迟、滤波确认延迟、通信传输延迟和执行器响应延迟确定系统总延迟,并根据当前电机电流限幅、母线电压状态和负载状况估计当前可用减速度。
9、根据所述平台当前速度、所述趋近速度、所述系统总延迟、所述当前可用减速度以及预设的安全余量,计算动态制动需求距离。具体地,所述动态制动需求距离包括以下组成部分:目标停止距离、趋近速度在系统总延迟内的继续趋近距离、平台以当前速度按当前可用减速度制动所需的减速距离、以及预设的安全余量。
10、s3):剩余制动余量计算与制动触发判断:根据步骤s1输出的滤波后的距离值与步骤s2计算得到的动态制动需求距离的差值,确定当前时刻的剩余制动余量,所述剩余制动余量用于量化当前时刻平台相对于目标的制动安全裕度。
11、将所述剩余制动余量与预设的进入阈值和退出阈值进行比较,以决定是否触发或退出制动;其中,所述退出阈值大于所述进入阈值,形成制动状态切换的滞回特性。
12、s4):基于状态机的速度控制与平滑过渡:根据步骤s3的判断结果,控制平台在多个工作状态之间切换并执行相应的速度控制策略,所述多个工作状态至少包括正常跟踪状态和动态制动状态。
13、在所述正常跟踪状态下,控制所述平台以基础速度变化斜率向目标速度渐进变化,实现速度的平滑过渡。
14、在所述动态制动状态下,根据所述剩余制动余量的大小动态调整减速斜率,其中所述剩余制动余量越小,所述减速斜率越大,即减速指令越强;当所述剩余制动余量恢复至所述退出阈值以上且连续满足预设周期数时,退出动态制动状态并返回正常跟踪状态。
15、在所述正常跟踪状态和所述动态制动状态下,当检测到所述实时数据或所述滤波后数据的可信度低于预设水平时,禁止所述平台加速,仅允许保持当前速度或减速。
16、基于趋近风险分级和动态制动余量的移动平台跟随控制系统,用于实现上述方法,所述系统包括:
17、传感器模块,配置为以预设周期获取跟随目标与移动平台之间的实时距离数据和角度数据,所述传感器模块优选为uwb定位模块;
18、数据处理模块,与所述传感器模块通信连接,配置为对所述实时距离数据和角度数据进行数据有效性判断、跳变检测与确认、以及加权滤波处理,输出滤波后的距离值、滤波后的角度值以及距离变化趋势;
19、制动余量计算模块,与所述数据处理模块通信连接,配置为根据连续两帧滤波后的距离值计算目标相对于平台的趋近速度,根据系统总延迟和当前可用减速度计算动态制动需求距离,并根据所述滤波后的距离值与所述动态制动需求距离的差值确定剩余制动余量;
20、状态机与控制模块,与所述制动余量计算模块通信连接,配置为根据所述剩余制动余量控制所述平台在正常跟踪状态和动态制动状态之间切换;在所述正常跟踪状态下以基础速度变化斜率向目标速度渐变,在所述动态制动状态下根据所述剩余制动余量动态调整减速斜率,其中所述剩余制动余量越小,所述减速斜率越大;以及,在数据可信度低于预设水平时禁止加速指令输出。
21、优选的,所述数据处理模块具体包括:
22、有效性判断单元,用于对数据帧进行帧格式校验、字段有效性检查和信号质量评估,剔除无效数据帧。
23、跳变检测单元,用于将当前有效帧的数据值与上一有效帧的数据值进行比较,当距离变化量超过距离跳变阈值或角度变化量超过角度跳变阈值时,通过连续预定数量帧的确认机制决定是否采纳新数据值。
24、加权滤波单元,用于根据目标距离增减趋势选择不同的距离平滑策略,以及根据角度变化量大小选择不同的角度平滑策略,对确认后的数据进行滤波输出。
25、优选的,所述状态机还包括空闲状态、停止状态和失联安全等待状态,其中:所述空闲状态为系统初始化后的等待状态;所述停止状态在平台进入预设停止范围时触发停车动作;所述失联安全等待状态在连续无有效数据的时间超过预设超时阈值时触发,所述平台在所述失联安全等待状态下执行柔性停止并等待数据恢复,且不执行基于最后已知方向的实体旋转搜索动作。
26、优选的,所述状态机在所述跟踪状态和所述制动状态之间设置不同的进入阈值和退出阈值,所述退出阈值大于所述进入阈值,形成滞回特性,以避免在阈值边界处发生状态反复切换。
27、与现有技术相比,本发明具有以下显著有益效果:
28、1、控制输入可靠且制动决策自适应,显著提升跟随安全性与响应及时性。
29、本发明首先通过数据有效性判断剔除无效帧、跳变检测与连续多帧确认机制滤除uwb信号偶发大幅跳变、以及根据距离增减趋势和角度变化量分别选择不同平滑策略的加权滤波,三级处理协同作用,为控制系统提供了稳定可靠的距离、角度及趋近趋势输入,从根本上避免了无效数据或跳变数据引发误动作。在此基础上,本发明提出的动态制动需求距离同时综合考虑了平台当前速度、目标趋近速度、全链路系统延迟、当前可用减速度及安全余量等多个动态因素,并以滤波后距离与制动需求距离的差值(即剩余制动余量)作为制动触发和强度调节的统一判据,使制动决策能够随实际风险动态自适应调整。相比现有固定阈值或固定预测时间的方案,本发明能够更早识别制动风险并执行与之匹配的减速动作,有效避免了制动过晚导致的碰撞风险或制动过早导致的跟随效率下降,大幅提升了跟随过程的安全性。
30、2. 多级平滑机制与滞回切换策略协同,有效降低机械冲击并消除状态振荡。
31、本发明在速度控制层面构建了双重平滑机制:正常跟踪状态下采用基础斜率向目标速度渐进变化,避免突发加速冲击;制动状态下根据剩余制动余量动态连续调节减速斜率(余量越小减速斜率越大),实现制动力度的无级平滑变化。同时,在跟踪状态与制动状态之间设置不同的进入阈值和退出阈值(退出阈值大于进入阈值),并结合连续多帧确认的退出条件,形成滞回特性。上述速度平滑机制与状态滞回切换机制协同作用,既避免了传统离散档位切换带来的速度突变和机械冲击,改善了乘坐舒适性并减少了电机磨损,又有效抑制了因数据微小波动导致的频繁状态跳转和速度振荡,系统运行稳定性显著提高。
32、3. 低算力需求保障工程易部署,且功能边界清晰利于系统集成。
33、本发明的核心算法均为确定性解析计算和简单逻辑判断,不依赖模型预测控制(mpc)等复杂优化算法或精确的目标运动模型辨识,可在mcu级嵌入式平台上实时运行,计算开销小、部署成本低,具有较强的工程实用价值和推广前景。同时,本发明在数据丢失或异常时的失联安全等待状态下仅执行柔性停止并持续监听,不包含基于最后方向的实体旋转搜索和重捕获功能,与独立的目标丢失后防丢失方案形成了清晰的功能边界,既避免了功能重叠造成的控制冲突,也便于与上层系统或其他功能模块协同配合,有利于模块化开发和系统集成。
34、上述说明仅是本发明技术方案的概述,为了能够更清楚了解本发明的技术手段,并可依照说明书的内容予以实施,以下以本发明的较佳实施例并配合附图详细说明如后。
版权声明:本站专利文献数据主要来源于国家知识产权局等公开渠道,由 技术新讯 进行整理汇编。页面设计及数据汇编的著作权归本站所有。全部内容仅供个人学习、科研评估与技术交流参考,严禁用于任何商业用途(包括但不限于直接生产制造、技术转让、倒卖数据等)。
免责条款:专利技术具有时效性,本站不对资料的绝对准确性、完整性和可实施性提供任何明示或暗示的担保。因参考本站内容造成的直接或间接损失,本站不承担相关法律责任。若您认为本站内容有侵权问题,请及时提供权属证明与我们联系,本站将在核实后依法处理。反馈意见:yyfuon@163.com