一种四足机器人机载相机稳像曝光协同控制方法及系统与流程

本发明涉及机器人视觉与数字图像处理，尤其涉及一种四足机器人机载相机稳像曝光协同控制方法及系统。

背景技术：

1、四足机器人在行走、奔跑等运动过程中，足端做具有周期性相位的往复运动，足端触地时的冲击力经机身结构传递至机载相机，使相机同时承受两类画质损伤：一类是几何抖动，即相机姿态随步态周期性快速变化，导致视频帧间画面晃动；另一类是运动模糊，即曝光时间窗内相机姿态或场景发生显著变化，导致单帧画面产生拖影。这两类画面损伤的物理来源相同，但现有技术往往采用孤立处理的手段。

2、针对几何抖动，现有方案包括机械云台增稳、纯视觉电子稳像以及基于imu的数字稳像。现有技术已利用步态相位择时触发拍摄，但仍依赖足端力传感器与机械云台。其中，基于imu的数字稳像通常对解算姿态做平滑处理（如以固定平滑系数做球面线性插值或低通滤波）得到参考姿态，进而矫正图像。现有技术多使用固定的平滑参数，少数参照已发生的姿态扰动做事后调节，属于反应式控制，难以应对四足机器人这种带有往复式、冲击式扰动的场景。

3、针对运动模糊，现有自动曝光技术多依据图像内容反馈调节曝光参数，反馈意味着滞后，且无法区分运动模糊与场景亮度变化；相机外触发同步成像技术虽已广泛用于工业检测，但触发时刻通常按固定周期生成，独立于机体运动。

4、从四足机器人的步态可以观察到，其运动具有周期性或短时周期性，足端触地冲击呈规律性出现，冲击基频随运动速度变化但在短时窗内可视为恒定。该先验信息在四足机器人运动控制领域已被广泛应用于运动规划与控制，但尚未被用于相机成像链路（触发时刻、曝光参数）与数字稳像链路（平滑参数调度）的协同预测控制。

5、综上，现有技术至少存在以下问题：其一，稳像平滑参数缺乏前馈性调度，固定平滑系数在冲击峰值期间会使矫正变换量触及图像裁剪边界，冲击后参考姿态追赶实际姿态，产生可见的视觉跳跃；其二，曝光控制与运动状态脱节，基于图像内容的自动曝光存在滞后，固定周期触发可能在冲击峰值时刻曝光，使单帧模糊与帧间抖动同时恶化；其三，稳像与曝光两条链路相互独立，未利用同一预测量进行协同控制，整体画质收益受限。

技术实现思路

1、本发明的目的是提供一种四足机器人机载相机稳像曝光协同控制方法及系统，针对四组机器人-相机-imu刚性连接，无机械云台情况下，仅利用imu单一测量数据源，基于步态冲击的周期性先验信息对足端冲击进行在线预测，并以同一预测量统一驱动稳像参数与成像参数的协同调节，通过前馈式手段同时抑制几何抖动与运动模糊，避免冲击过程中因参数设置不当导致的视觉跳跃，并保证变帧率输出的时序一致性。

2、为实现上述目的，本发明提供了一种四足机器人机载相机稳像曝光协同控制方法，包括以下步骤：

3、步骤s1、外触发采集与时空对齐：通过离线标定的方式，获取惯性测量单元的测量数据以及外触发脉冲的时间戳，并确定任一帧图像的曝光中心时刻，将惯性测量单元的测量数据对齐至曝光中心时刻；

4、步骤s2、惯性测量单元标定与预处理：对测量数据进行确定性误差补偿，并进行姿态解算获得惯性测量单元的姿态，根据姿态去除加速度数据中的重力分量，获得动态加速度；

5、步骤s3、足端冲击在线预测：由动态加速度构造反映足端冲击的冲击信号序列；对冲击信号序列进行周期性分析获得冲击基频估计；对冲击信号序列进行冲击事件检测获得冲击事件时刻；以最近一次冲击事件时刻为相位零点计算冲击相位，并由冲击相位外推获得下一次足端冲击的预测时刻；将检测到的冲击事件时刻与预测时刻之间的相位误差反馈用于修正冲击基频估计，构成频率-相位闭环跟踪；根据冲击信号序列估计冲击强度与预测置信度；

6、步骤s4、修正增益预测式门控：构造冲击接近度函数，冲击接近度函数随当前时刻与预测时刻之间的接近程度在0至1之间变化，按冲击接近度函数在预测冲击到达前预先压低姿态解算的修正增益；

7、步骤s5、平滑系数双层限幅自适应平滑：按冲击接近度函数与冲击强度调节姿态平滑的平滑系数，对姿态做平滑处理获得参考姿态；

8、步骤s6、图像矫正：根据姿态与参考姿态之差计算矫正变换，对相机采集的图像进行矫正；

9、步骤s7、触发控制：以预测时刻为中心设置冲击避让时间区间，估计名义触发时刻，并在名义触发时刻附近的允许调整区间内选取实际触发时刻，使扣除触发延迟与半个曝光时长后的曝光中心落入相邻冲击避让时间区间之间的平静窗口；

10、步骤s8、曝光参数分配：根据冲击基频估计与冲击强度调节曝光时长与增益iso；

11、步骤s9、失效回退：当预测置信度低于预设门限时，切换为定帧率自由触发、固定平滑系数与默认自动曝光策略，待预测置信度恢复后退出，切换过程设置滞回。

12、优选的，在步骤s1中，外触发采集与时空对齐，具体过程如下：

13、设外部触发脉冲的时间戳为，相机从接收到脉冲到实际曝光的时间延迟为，曝光持续时间为，则当前帧的曝光开始、曝光结束与曝光中心时刻，如下所示：

14、；

15、；

16、；

17、其中，为曝光时长；触发延迟与曝光时长由相机资料给出或离线标定获得；将imu数据插值或积分至曝光中心时刻，并使用曝光中心时刻对应的解算姿态作为稳像的基姿态。

18、优选的，在步骤s2中，惯性测量单元imu标定与预处理，具体过程如下：

19、步骤s21、在离线标定阶段，对imu进行六面静止标定，获得加速度计与陀螺仪的确定性误差参数，包括零偏和、刻度系数矩阵与安装失准矩阵；对imu静止数据做allan方差分析，获得随机误差参数，包括随机游走系数与零偏不稳定性；对imu与相机进行外参标定，获得imu坐标系至相机坐标系的旋转矩阵，及安装杆臂向量，并完成时间对齐；

20、步骤s22、在线运行阶段，将加速度计与陀螺仪的确定性误差参数对实时采集的imu数据进行逐点补偿，如下所示：

21、；

22、；

23、其中，为实时检测到的加速度；为补偿后的加速度；为实际采集到的角速度；为补偿后的角速度，和为包含刻度系数与失准修正的补偿矩阵；

24、将allan标定获得的随机误差参数，用于设定带通滤波器参数、事件检测自适应门限系数及融合噪声协方差；

25、步骤s23、基于补偿后的、数据进行姿态解算，实时输出imu坐标系至世界坐标系的姿态；

26、首先，将重力变换到imu坐标系，如下所示：

27、；

28、其中，为世界坐标系的重力向量；为变换到imu坐标系中的重力向量；表示imu坐标系向世界坐标系的转换矩阵，其转置即其逆；

29、然后，在imu坐标系内扣除重力分量后，再经旋转矩阵变换到相机坐标系，获得动态加速度，如下所示：

30、；

31、其中，为动态加速度；

32、当imu敏感中心与相机光心不重合时，机体旋转会在相机处引入附加加速度，补偿后的动态加速度完整形式，如下所示：

33、；

34、其中，为机体角加速度，对差分获得；为切向项；为向心项；表示安装杆臂。

35、优选的，在步骤s3中，冲击基频是指足端冲击事件列的基频，四足机器人冲击事件列与步态相关：trot步态每个步态周期包含2次对角足冲击，冲击基频为步频的2倍；walk步态每个步态周期包含4次冲击事件；当冲击事件在步态周期内非均匀分布时，直接以各次冲击事件时刻序列替代单一基频进行冲击预测与曝光调度；

36、步骤s31、根据相机坐标系下的动态加速度构造冲击信号序列，如下所示：

37、；

38、其中，表示带通滤波；

39、步骤s32、对冲击信号序列进行周期性分析，获得冲击基频估计；

40、首先，对冲击信号序列施加不同长度的滑动分析窗，窗随采样滚动更新；其次，对每一档窗内序列分别进行周期性分析，在预设频带内搜索主峰并对主峰位置做抛物线插值，得到各窗的冲击基频估计及其主峰显著度；然后，按显著度对各窗估计加权融合，输出融合后的冲击基频估计；当各窗主峰显著度均低于预设门限时，冻结输出并保持最近一次有效值；对融合后的冲击基频估计施加一阶低通并限制变化率；

41、步骤s33、对冲击信号序列进行冲击事件检测获得冲击事件时刻；

42、当冲击信号序列超过自适应门限且为局部极大值时，判定发生一次足端冲击事件；对峰附近采样点做抛物线插值，获得亚采样精度的事件时刻；检测到一次事件后的预设间期内不再响应新的事件，以避免单次冲击的振荡引起重复计数；

43、步骤s34、以最近一次确认的冲击事件时刻为相位零点，计算当前冲击相位，如下所示：

44、；

45、其中，为平滑后的冲击基频估计；按线性外推获得未来δ时刻的相位，如下所示：

46、；

47、其中，δ为预测提前量，不大于当前冲击周期估计值；

48、由冲击相位外推获得下一次冲击事件的预测时刻，如下所示：

49、；

50、步骤s35、将每次检测到的冲击事件时刻与由相位外推预测的事件时刻比较，得到相位误差；当不超过预设容差时保持相位连续，并将按比例反馈修正冲击基频估计，构成频率-相位闭环跟踪；当超过容差时，以重置相位零点；

51、当超过预设时长仍未检测到新的冲击事件时，判定事件漏检，保持按当前继续外推相位；连续多个冲击周期无有效事件时，置失锁标志并冻结输出；

52、步骤s36、取n个冲击周期冲击事件峰值的滑动平均或包络作为当前强度估计，记为，并按趋势外推下一周期强度，归一化后记为；

53、以若干周期预测时刻与实测冲击事件时刻偏差的滑动统计度量预测可靠性，映射为预测置信度。

54、优选的，在步骤s4中，修正增益预测式门控，具体过程如下：

55、采用姿态解算算法，估计实时姿态，其更新方程，如下所示：

56、；

57、其中，为补偿后的陀螺测量值；为归一化加速度计测量值；为由加速度计重力参考构造的姿态误差函数梯度；为修正增益；

58、对修正增益实施预测式门控，如下所示：

59、；

60、其中，为正常工况修正增益；为冲击接近度函数，取值[0,1]，在预测冲击时刻取1、在平静相位趋近0，公式定义为：

61、；

62、其中，为冲击相位到最近冲击相位的角距离；为高斯窗函数；为门控深度。

63、优选的，在步骤s5中，平滑系数双层限幅自适应平滑，具体过程如下：

64、对解算姿态做平滑以分离意图运动与抖动，基础形式为四元数球面线性插值，如下所示：

65、；

66、其中，为平滑系数；

67、按预测冲击对实施双层限幅自适应调节，如下所示：

68、；

69、；

70、；

71、其中，和分别是平滑系数的最大最小取值；为强度系数；为按外推冲击相位计算的冲击接近度函数值；为预测提前量；为冲击强度增益系数；为增益调节的上限；为预测的冲击强度；为基础上限；、分别为下限截断区间和上限截断区间；为平滑系数单步最大变化量；

72、在步骤s6中，根据姿态与参考姿态之差计算矫正变换，对相机采集的图像进行矫正，具体过程如下：

73、按实际姿态与参考姿态之差计算相对旋转，进而计算矫正变换并仿射图像，如下所示：

74、；

75、其中，为实际姿态与参考姿态之间的相对旋转；为时刻的参考姿态旋转矩阵；为时刻的实际姿态旋转矩阵；为相机内参矩阵。

76、优选的，在步骤s7中，触发控制的具体过程如下：

77、步骤s71、按递归方式生成第k帧的名义触发时刻，如下所示：

78、；

79、其中，为第k帧的设定输出帧率；

80、冲击避让区间与平静窗口：以各预测冲击时刻为中心定义冲击避让区间，如下所示：

81、；

82、其中，、按冲击瞬态持续时间统计设定；

83、相邻避让区间之间构成平静窗口，距两侧冲击最远的时刻，冲击相位π处为其中心，如下所示：

84、；

85、；

86、其中，表示足端冲击周期；

87、步骤s72、在名义触发时刻附近设置允许调整区间，如下所示：

88、；

89、其中，不大于当前冲击周期估计值的1/4，以保证调整不跨越相邻平静窗口；

90、在允许调整区间内选取触发时刻，使曝光中心对准最近的平静窗口中心，如下所示：

91、；

92、其中，为距名义触发时刻最近的平静窗口序号；为触发延迟；为曝光时长；表示将括号内时刻投影至允许调整区间；括号内时刻的含义为扣除触发延迟与半个曝光时长后提前触发，使曝光中心恰好落在平静窗口中心；为扣除触发延迟与半个曝光时长后提前触发，使曝光中心恰好落在平静窗口中心；

93、步骤s73、不可行工况的降级处理；

94、当允许调整区间被避让区间完全覆盖，无法找到满足避让约束的触发时刻时，按名义触发时刻触发，同时执行降级策略：按缩短曝光时长并联动提升iso，并增大对应的平滑系数。

95、优选的，在步骤s8中，根据冲击基频估计与冲击强度调节曝光时长与增益iso，具体过程如下：

96、运动模糊的像素量近似正比于曝光时间内像面角位移，如下所示：

97、；

98、其中，为运动模糊的像素量；为曝光窗预期角速度；为像素焦距；

99、由归一化冲击强度与冲击基频估计推算，如下所示：

100、；

101、其中，a为离线标定的比例系数；

102、给定模糊预算，闭环求解曝光时长上限，如下所示：

103、；

104、曝光时间缩短造成的曝光量不足由iso补偿，如下所示：

105、；

106、其中，为相机基准感光度，和分别为相机感光度调节的下限和上限。

107、优选的，在步骤s9中，当预测置信度低于预设门限时：触发切换为定帧率自由运行，平滑系数恢复固定值，曝光参数恢复默认自动曝光策略；待连续若干冲击周期高于门限后自动退出回退模式。

108、一种四足机器人机载相机稳像曝光协同控制系统，用于执行上述一种四足机器人机载相机稳像曝光协同控制方法，包括：刚性安装于相机模组上的六轴惯性测量单元imu、支持外触发模式的全局快门外触发相机、机载计算单元；

109、其中，机载计算单元包括冲击在线预测模块、稳像支路与曝光控制支路；

110、imu安装位置尽量靠近相机光心且与相机刚性连接；触发脉冲由机载计算单元或专用定时器产生；imu与相机触发系统共用同一硬件时钟基准；离线标定imu-相机外参旋转矩阵与相机内参k。

111、因此，本发明采用上述一种四足机器人机载相机稳像曝光协同控制方法及系统，有益效果如下：

112、（1）本发明以一次冲击预测统一驱动稳像链路与成像链路，将几何抖动与运动模糊这两类同源损伤纳入统一的预测控制框架，冲击到达前完成参数调度，克服了反应式调节的固有滞后，冲击期间画面无矫正量顶格跳变，整体画质收益大于两条链路独立优化之和。

113、（2）本发明通过触发相位锁定平静窗口，使曝光窗避开冲击瞬态，从物理源头同时减轻抖动与模糊；触发相位锁定生效时曝光窗位于平静相位，预期角速度显著降低，曝光时长可相应放宽，图像信噪比受益，体现出两条支路共享预测量的协同收益。

114、（3）本发明通过修正增益预测式门控与平滑系数双层限幅调度，在冲击期间保护姿态基准不受冲击瞬态污染，并将矫正变换量约束在裁剪能力范围内，避免冲击后参考姿态追赶造成的视觉跳跃；置信度回退机制保证急停、转身、打滑等步态紊乱工况下系统行为可预期。

115、（4）本发明外触发时戳结合经标定的触发延迟与曝光时长直接给出帧曝光中心时刻，实现imu测量与图像曝光的精确时间对齐，消除imu-图像时间偏差这一稳像残差的主要来源，并保证变帧率输出的时序一致性。

116、（5）本发明不依赖机械云台与足端力传感器，仅需imu与相机本体的外触发接口，适用于低成本、无云台的轻量化四足机器人平台。

117、下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。