一种无人救援引导机器人的激光投射控制方法及系统与流程
本发明涉及激光投射控制，尤其涉及一种无人救援引导机器人的激光投射控制方法及系统。
背景技术：
1、近年来，随着无人平台与光电技术的快速发展，激光投射控制在应急救援、野外搜救及复杂灾害环境下的目标指示与引导中发挥着日益重要的作用，救援场景通常伴随浓烟、扬尘、多光谱干扰以及载体的动态位姿变化，对激光投射的实时性、环境适应性与指向稳定性提出了较高要求，传统激光投射控制方法大部分面向室内或者洁净环境设计，在角度解算、运动补偿与功率驱动策略难以充分适应开放空间及变衰减介质下的复杂工况，因此，构建一套融合全局位姿校正、动态前馈补偿与环境自适应调节的激光投射控制方案，对于提升救援引导装备在恶劣条件下的作业能力具有重要意义。
2、现有激光投射控制方案在实际工程应用中仍存在技术制约，具体体现在，目标角度解算主要基于单一坐标系转换，较少同时考虑车体航向角变化与振镜安装偏移的综合影响，初始指向精度存在提升空间；运动补偿环节大多数采用静态角度设定，没有充分利用车体实时角速率信息进行动态修正，高频扰动下的指向同步性有待改善；驱动输出通常设定为固定功率值，没有根据实际环境中烟雾浓度与颗粒度所引起的光学衰减特性进行动态适配，不同工况下的穿透一致性难以保证；反馈控制环节普遍采用固定参数调节方式，在杂散光波动或信噪比变化条件下，比例增益与积分环节的响应特性尚不能充分适应干扰变化，控制稳定性有待进一步增强，这些因素共同制约了现有方案在复杂救援环境下的投射精度与可靠性。
技术实现思路
1、本发明解决的技术问题是：现有激光投射控制在动态扰动下光轴指向滞后、烟雾衰减环境下激光穿透功率无法自适应补偿，以及杂散光干扰下反馈控制抗干扰能力不足。
2、为解决上述技术问题，本发明提供如下技术方案：一种无人救援引导机器人的激光投射控制方法，包括以下步骤：
3、步骤s1，对目标全局坐标进行坐标转换，生成振镜目标角度向量后，执行粗扫门限判定，得到首次探测角度，基于首次探测角度执行精扫峰值定位，得到精锁定初始角度；
4、步骤s2，基于精锁定初始角度生成前馈合成角度，根据烟雾衰减放大因子对激光器的驱动电流进行指数补偿后限幅，得到限幅后的驱动电流，通过限幅后的驱动电流驱动激光器；
5、步骤s3，基于各通道回波强度和各通道预期回波强度参考值，计算各通道归一化回波强度偏差比，根据各通道归一化回波强度偏差比分别计算杂散光干扰因子和二维角域偏差向量，根据二维角域偏差向量和杂散光干扰因子对比例增益对角矩阵进行动态调度后执行负反馈控制，得到振镜微调控制量，根据前馈合成角度与振镜微调控制量进行计算，得到更新后的振镜角度。
6、作为本发明所述的一种无人救援引导机器人的激光投射控制方法的一种优选方案，其中：步骤s1具体包括：
7、步骤s101，通过无人救援引导机器人获取目标全局坐标、本车全局位姿数据以及振镜与车体之间的安装偏移参数；
8、根据本车全局位姿数据构建本车全局位姿齐次变换矩阵；
9、根据安装偏移参数构建振镜安装偏移齐次变换矩阵；
10、将目标全局坐标的笛卡尔向量扩展为四维齐次坐标向量，得到扩展后的齐次坐标向量；
11、对齐次坐标向量进行齐次坐标变换，得到振镜坐标系下的齐次坐标向量；
12、提取该齐次坐标向量的前三个分量，将前三个分量分别作为x轴分量、y轴分量和z轴分量，构成振镜坐标系下的目标笛卡尔位置向量；
13、根据目标笛卡尔位置向量的x轴分量和y轴分量，采用四象限反正切函数进行计算，得到方位角角度值；
14、根据x轴分量和y轴分量计算水平径向分量，根据z轴分量与水平径向分量，采用四象限反正切函数进行计算，得到俯仰角角度值。
15、合并方位角角度值与俯仰角角度值，得到振镜目标角度向量。
16、作为本发明所述的一种无人救援引导机器人的激光投射控制方法的一种优选方案，其中：步骤s1还包括：
17、步骤s102，将振镜目标角度向量作为中心，生成离散角度网格，按螺旋顺序遍历离散角度网格中各角度点；
18、每遍历至一个角度点，控制振镜偏转至该角度点并采集该角度点的各通道回波强度，根据各通道回波强度计算综合回波强度；
19、根据综合回波强度与预设目标探测门限进行粗扫门限判定，具体包括：
20、若当前角度点对应的综合回波强度大于预设目标探测门限，则记录当前角度点为首次探测角度，终止遍历并输出该首次探测角度；
21、若当前角度点对应的综合回波强度小于等于预设目标探测门限，则继续遍历下一个角度点；
22、若遍历完离散角度网格中全部角度点后，全程对应的综合回波强度均小于等于预设目标探测门限，则判定未捕获有效回波，得到捕获失败标志并终止。
23、作为本发明所述的一种无人救援引导机器人的激光投射控制方法的一种优选方案，其中：步骤s1还包括：
24、步骤s103，将首次探测角度作为中心，生成离散精扫网格，按螺旋顺序遍历离散精扫网格中各角度点；
25、设置暂存强度变量并初始化为0，设置暂存角度变量并初始化为首次探测角度；
26、每遍历至一个角度点，控制振镜偏转至该角度点并计算该角度点的综合回波强度；
27、根据综合回波强度与暂存强度变量进行精扫峰值判定，具体包括：
28、若综合回波强度大于暂存强度变量，则将暂存强度变量更新为综合回波强度，并将暂存角度变量更新为角度点；
29、若综合回波强度小于等于暂存强度变量，则保持暂存强度变量和暂存角度变量不变；
30、遍历完离散精扫网格中全部角度点后，将暂存角度变量确定为精锁定初始角度，并输出捕获成功标志。
31、作为本发明所述的一种无人救援引导机器人的激光投射控制方法的一种优选方案，其中：步骤s2具体包括：
32、步骤s201，获取本机imu实时角速率；
33、imu实时角速率包括绕车体x轴的横滚角速率和绕车体y轴的俯仰角速率；
34、计算横滚角速率与预测窗口的乘积，得到横滚预补偿角；
35、计算俯仰角速率与预测窗口的乘积，得到俯仰预补偿角；
36、将精锁定初始角度分别与俯仰预补偿角和横滚预补偿角进行合成，得到前馈合成角度。
37、作为本发明所述的一种无人救援引导机器人的激光投射控制方法的一种优选方案，其中：步骤s2还包括：
38、步骤s202，获取目标投射距离、烟雾浓度、颗粒度、预设激光波长、预设目标维持光功率、预设激光器光电转换效率以及驱动电流；
39、根据烟雾浓度和颗粒度计算烟雾衰减放大因子，烟雾衰减放大因子在零烟雾浓度下的取值为1；
40、计算预设洁净空气基准衰减系数与烟雾衰减放大因子的乘积，得到实际衰减系数；
41、计算实际衰减系数与目标投射距离的乘积，得到衰减指数；
42、根据衰减指数进行计算，得到衰减补偿因子；
43、计算预设目标维持光功率与预设激光器光电转换效率的比值，得到基准驱动电流；
44、计算基准驱动电流与衰减补偿因子的乘积，得到修正后驱动电流，对修正后驱动电流进行限幅，得到限幅后的驱动电流。
45、作为本发明所述的一种无人救援引导机器人的激光投射控制方法的一种优选方案，其中：步骤s3具体包括：
46、步骤s301，对各通道执行信号有效性检测，得到有效性检测后的各通道波段置信权重；
47、有效性检测具体包括：
48、若通道回波强度小于预设信号有效门限，则将该通道对应的波段置信权重置为零；
49、若当前通道回波强度大于等于预设信号有效门限，则保持该通道对应的波段置信权重不变；
50、将有效性检测后的各通道波段置信权重进行归一化处理，归一化处理后的各通道波段置信权重；
51、计算各通道预期回波强度参考值和各通道回波强度的比值后进行归一化，得到各通道回波强度偏差比：
52、计算各通道预期回波强度参考值与对应通道回波强度的差值，得到各通道回波强度差值；
53、计算各通道预期回波强度参考值与预设安全常数的和值，得到各通道分母值；
54、计算各通道回波强度差值与对应通道分母值的比值，得到各通道归一化回波强度偏差比；
55、计算归一化处理后的各通道波段置信权重与各通道归一化回波强度偏差比的乘积并相加，得到杂散光干扰因子；
56、根据各通道归一化回波强度偏差比，采用矩阵乘法进行角域偏差映射，得到二维角域偏差向量。
57、作为本发明所述的一种无人救援引导机器人的激光投射控制方法的一种优选方案，其中：步骤s3还包括：
58、步骤s302，获取历史偏差累积向量、基准增益对角矩阵和固定积分系数对角矩阵；
59、根据二维角域偏差向量计算二维角域偏差向量的偏差范数；
60、根据偏差范数和杂散光干扰因子，对比例增益对角矩阵进行动态调度，表达式为：
61、；
62、其中，表示动态调度后的比例增益对角矩阵，表示基准增益对角矩阵，表示逐元素乘法符号，表示二维角域偏差向量的偏差范数，表示杂散光干扰因子的绝对值，表示预设偏差幅度抑制系数，表示预设杂散光干扰抑制系数；
63、将二维角域偏差向量叠加至历史偏差累积向量，得到原始积分累积向量；
64、对原始积分累积向量进行限幅处理，得到限幅后积分累积向量。
65、作为本发明所述的一种无人救援引导机器人的激光投射控制方法的一种优选方案，其中：步骤s3还包括：
66、步骤s303，根据动态调度后的比例增益对角矩阵、二维角域偏差向量、固定积分系数对角矩阵和限幅后积分累积向量，执行负反馈控制，得到振镜微调控制量，表达式为：
67、；
68、其中，表示振镜微调控制量，表示动态调度后的比例增益对角矩阵，表示二维角域偏差向量，表示固定积分系数对角矩阵，表示限幅后积分累积向量；
69、将前馈合成角度与振镜微调控制量相加，得到更新后的振镜角度；
70、并根据更新后的振镜角度生成振镜驱动指令，控制振镜偏转至该振镜角度，完成激光投射指向的修正。
71、一种无人救援引导机器人的激光投射控制系统，其中包括定位模块、前馈模块和反馈模块；
72、定位模块，对目标全局坐标进行坐标转换，生成振镜目标角度向量后，执行粗扫门限判定，得到首次探测角度，基于首次探测角度执行精扫峰值定位，得到精锁定初始角度；
73、前馈模块，基于精锁定初始角度生成前馈合成角度，根据烟雾衰减放大因子对激光器的驱动电流进行指数补偿后限幅，得到限幅后的驱动电流，通过限幅后的驱动电流驱动激光器；
74、反馈模块，基于各通道回波强度和各通道预期回波强度参考值，计算各通道归一化回波强度偏差比，根据各通道归一化回波强度偏差比分别计算杂散光干扰因子和二维角域偏差向量，根据二维角域偏差向量和杂散光干扰因子对比例增益对角矩阵进行动态调度后执行负反馈控制，得到振镜微调控制量，根据前馈合成角度与振镜微调控制量进行计算，得到更新后的振镜角度。
75、本发明的有益效果：通过建立基于齐次坐标变换的多级坐标映射，在目标定位时综合校正了车体航向角与振镜安装偏移对初始指向精度的影响；通过imu实时角速率的前馈补偿，在运动补偿时有效减小了车体高频扰动下的光轴指向滞后效应；通过指数型电流补偿，使激光器驱动电流能够随烟雾浓度与颗粒度变化进行自适应调节，保障了不同衰减环境下的穿透功率稳定性；通过引入杂散光干扰因子，并根据杂散光干扰因子对比例增益对角矩阵进行动态调度，配合积分限幅处理，在杂散光波动下增强了控制过程的抗干扰能力与收敛稳定性，实现了激光投射控制在动态扰动与复杂介质环境下能够维持稳定的光轴锁定与平滑跟踪，有效提升了救援引导装置在恶劣工况下的工程适用性与任务执行可靠性。