(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210493513.5 (22)申请日 2022.04.26 (71)申请人 西北工业大 学 地址 710072 陕西省西安市友谊西路127号 (72)发明人 张立川　赵荞荞　潘光　刘禄　 柏书昌　朱梓霄　 (74)专利代理 机构 西安凯多 思知识产权代理事 务所(普通 合伙) 61290 专利代理师 云燕春 (51)Int.Cl. G06T 7/73(2017.01) G06T 7/246(2017.01) G06T 7/207(2017.01) G06T 7/13(2017.01) B63C 11/52(2006.01) (54)发明名称 一种机器人运动模式估计方法及装置 (57)摘要 本发明一种机器人运动模式估计方法及装 置， 属于仿生机器人技术领域； 首先在水下仿生 机器人上布 放四点光源信标， 标定机器人身体纵 轴与胸鳍的位置； 然后， 通过图像采集单元采集 水下仿生机器人的运动图像， 再进行预处理， 得 到清晰的光源目标特征； 对目标光源在图像上的 目标位置参数进行检测， 得到目标光源的图像坐 标； 最后， 计算水下仿生机器人左右两侧胸鳍鳍 条与身体纵轴之间的夹角、 并比较， 得到水下仿 生机器人的运动模式估计结果。 本发 明采用水下 视觉及光源信标的方法得到了前方水下仿生机 器人的运动模式， 简化了水下仿生机器人间的通 信， 同时也为后方机器人提供运动反馈信息， 对 于后续水下仿生机器人集群编队研究奠定了基 础。 权利要求书2页 说明书12页 附图6页 CN 114820795 A 2022.07.29 CN 114820795 A 1.一种机器人运动模式估计方法， 应用于胸鳍拍动水下仿生机器人， 其特征在于具体 步骤如下： 步骤1： 在水下仿生机器人上布放 四点光源信标， 用于标定机器人身体纵轴与胸鳍的位 置； 步骤2： 通过图像采集单 元采集水 下仿生机器人的运动图像； 步骤3： 对 采集到的水 下仿生机器人运动图像进行 预处理， 得到清晰的光源目标 特征； 步骤4： 对目标光源在图像上的目标位置参数进行检测， 得到目标光源的图像坐标； 步骤5： 计算水下仿生机器人左右两侧胸鳍鳍条与身体纵轴之间的夹角、 并比较， 得到 水下仿生机器人的运动模式估计结果： 若相等， 则判定水下仿生机器人为主动推进模式， 若 不相等， 则为 转弯模式。 2.根据权利要求1所述水下仿生机器人运动模式估计方法， 其特征在于： 所述步骤1中， 所述四点光源信标包括一个蓝 色灯和三个绿色光源灯； 蓝 色光源灯位于水下仿生机器人的 机身上方， 并位于机器人纵轴上； 一个绿色光源灯位于水下仿生机器人的机身 下方， 并位于 机器人纵轴上； 另外 两个绿色光源灯分别位于胸鳍末端。 3.根据权利要求1所述水下仿生机器人运动模式估计方法， 其特征在于： 所述步骤2中， 图像采集单 元为双目相机 。 4.根据权利要求1所述水下仿生机器人运动模式估计方法， 其特征在于： 所述步骤3 中， 对采集到的水下仿生机器人运动图像进行预处理包括， 颜色色彩空间转换、 图像平滑处理 和图像边 缘检测， 通过图像预处 理操作过 滤掉非目标 特征。 5.根据权利要求1所述水下仿生机器人运动模式估计方法， 其特征在于： 所述图像平滑 处理采用中值滤波的方法模糊图像并降低灰度图像的噪声。 6.根据权利要求1所述水下仿生机器人运动模式估计方法， 其特征在于： 所述图像边缘 检测采用can ny边缘检测对目标图像进行处 理。 7.根据权利要求1所述水下仿生机器人运动模式估计方法， 其特征在于： 所述步骤4中， 对目标光源在图像上的目标位置参数检测是基于最小二乘的目标光源检测算法， 能够剔除 由于水面倒影产生的虚假 光源； 首先， 使用边 缘提取算法在平 滑处理后的图像上对目标的边 缘特征进行提取； 然后， 剔除所提取的目标边 缘特征中的噪声和亮点； 最后， 选取目标光源圆心Y坐标最小的四个点作为图像坐标。 8.根据权利要求1所述水下仿生机器人运动模式估计方法， 其特征在于： 所述步骤5 中， 计算图像中水 下仿生机器人左右两侧夹角的方法： 首先， 定义机器人上方、 并位于身体纵轴上的光源信标灯圆心为坐标为Ob＝(xb,yb)， 位 于机器人下方、 并位于身体纵轴上的光源信 标灯圆心为坐标为Og＝(xg,yg)， 则由这两个光 源组成的直线几何中心坐标Oc＝(xc,yc)表达式为 定义剩余两个标定两侧胸鳍的信标光源灯在所拍摄图像上的圆心坐标为Og1＝(xg1, yg1)， Og2＝(xg2,yg2)。 以Oc为基点， 向量 表示为权　利　要　求　书 1/2 页 2 CN 114820795 A 2设计夹角计算 函数 则向量 和向量 的夹角 θ1计算公式为 θ1＝f(xc‑xb,yc‑yb)‑f(xc‑xg1,yc‑yg1)     (4) 同理， 向量 和向量 的夹角 θ2计算公式为 θ2＝f(xc‑xb,yc‑yb)‑f(xc‑xg2,yc‑yg2)     (5) 根据水下仿生机器人的运动特点， 当水下仿生机器人处于主动推进模式时， 由识别到 的光源圆心 坐标构成的向量夹角 θ1＝θ2， 当水下仿生机器人 处于转弯模式时， 向量夹角 θ1≠ θ2。 9.一种应用权利要求1 ‑8任一项所述水下仿生机器人运动模式估计方法的装置， 其特 征在于： 包括四点 光源信标灯、 图像采集单 元、 图像处 理单元和目标位置解析 单元； 所述四点 光源信标灯用于标定 机器人身体纵轴与胸鳍的位置， 作为目标图像； 所述图像采集单 元用于实时采集水 下仿生机器人的运动图像； 所述图像处理单元用于对目标图像在运动图像上进行预处理， 得到清晰的光源目标特 征； 所述目标位置解析单元用于对目标光源在图像上的目标位置参数进行检测， 得到目标 光源的图像坐标； 并计算水下仿生机器人左右两侧胸鳍鳍条与身体纵轴之间的夹角、 并比 较， 得到得到水 下仿生机器人的运动模式估计结果。 10.根据权利要求9所述水下仿生机器人运动模式估计方法的装置， 其特征在于： ： 所述 装置应用于仿生机器人集群运动控制中， 集群中任一仿生机器人通过图像采集单元采集前 方仿生机器人的运动模式， 并将运动模式信号传输至控制系统， 由控制系统输出指令， 控制 后方仿生机器人作出运动响应。权　利　要　求　书 2/2 页 3 CN 114820795 A 3