(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210737308.9 (22)申请日 2022.06.27 (71)申请人 上海交通大 学 地址 200240 上海市徐汇区华 山路1954 号 (72)发明人 刘亚东　严英杰　姜骞　江秀臣　 (74)专利代理 机构 合肥拓信专利代理事务所 (普通合伙) 34251 专利代理师 徐海燕 (51)Int.Cl. B25J 9/16(2006.01) B25J 19/02(2006.01) (54)发明名称 一种变电站巡 检机器人位姿误差 计算方法 (57)摘要 本发明公开了一种变电站巡检机器人位姿 误差计算方法， 涉及电站运维技术领域， 包括建 立巡检机器人位姿计算模型； 计算巡检机器人位 姿误差； 对巡检机器人进行重新定位， 对所述巡 检机器人位姿误差进行补偿， 将所述巡检机器人 导航到预定巡检点。 能够对变电站巡检机器人每 次在预设的停留位置与实际的停留位置的导航 误差进行计算， 并根据计算结果对变电站巡检机 器人的当前位置进行校准， 使巡检机器人到达标 定停靠点并以标定的云台角度拍摄巡检图像， 避 免巡检机器人， 沿着预定的巡检 路线在各个巡检 点上停留是， 导航误差会逐渐累积， 导致预先标 定的目标检测图像和实际检测图像之间的像素 偏差将线性增 加。 权利要求书2页 说明书8页 附图1页 CN 115157243 A 2022.10.11 CN 115157243 A 1.一种变电站巡检机器人位姿误差计算方法， 其特 征在于， 包括： 建立巡检机器人位姿计算模型； 基于所述巡检机器人位姿计算模型， 计算巡检机器人当前所在巡检点与预定巡检点之 间的位姿误差； 对巡检机器人进行重新定位， 对所述巡检机器人位姿误差进行补偿， 将所述巡检机器 人导航到预定 巡检点。 2.如权利要求1所述的变电站巡检机器人位姿误差计算方法， 其特征在于： 获取变电站 三维模型地图， 并预先在所述变电站三维模型地图标定巡检路线， 同时标定所述巡检路线 上的巡检点， 并标定各个所述巡检点上巡检机器人云台的位姿和摄 像机的位姿。 3.如权利要求1所述的变电站巡检机器人位姿误差计算方法， 其特征在于： 所述巡检机 器人位姿计算模型包括： 将巡检机器人的位姿用一个4 ×4矩阵表示， 其计算表达式为： 其中， p表示巡检机器人的位姿， SE(3)表示特殊的欧几里得群， R∈R3 ×3表示正交旋转 矩阵， t∈R3 ×1表示平移矢量矩阵也就是平移向量， 为了方便起见， 我们用一个两元组(R， t)来等价表示巡检机器人的位姿p； 并将Tij＝(Rij， tij)表示从Pi到Pj的变 换， 即Pj＝TijPi， 也就是巡检机器 人的位姿p从 i点到j点， 在一个平面 坐标系中其所在坐标的变换。 4.如权利要求3所述的变电站巡检机器人位姿误差计算方法， 其特 征在于： 在拍摄状态下， 巡检机器人当前的底盘位姿 和底盘的位移误差向量th的表达式为： 其中， 表示当前的底盘位姿， 也就是巡检机器人目前的位姿， Ph表示底盘在上一的巡 检点的底盘位姿， th表示底盘 的位移误差向量， 也就是巡检机器人从上一个巡检点到下一 个预定的巡检点之间的位移与实际到 达的下一个预定的巡检点 误差向量。 5.如权利要求 4所述的变电站巡检机器人位姿误差计算方法， 其特 征在于： 在变电站三维模型地图中， 以巡检机器人的位置为坐标系， 其中巡检机器人(thx， thy， thz)分别为所在位置的X轴、 Y轴和Z轴的坐标参数， 则巡检机器人位姿误差的向量th＝(thx， thy， thz)T。 6.如权利要求5所述的变电站巡检机器人位姿误差计算方法， 其特 征在于： 将摄像机预定的位置看做一个在变电站三维模型中的三维点Xc， Xc的三维坐标就为Xc ＝(Xc， Yc， Zc)， 对应于摄 像机三维模型中的Xc， 其计算表达式为： 其中， 表示摄像头预定位置的位姿， 表示摄像头实际位置的位姿， 而三维点实际 的位置的误差向量为 所述巡检机器 人工作时包 括校准状态和拍摄状态， 校准状态下摄 像机位姿Pc和拍摄 状态下的摄 像机位姿 的计算表达式为：权　利　要　求　书 1/2 页 2 CN 115157243 A 2然后， 摄像机位姿Pc的误差Tc的计算表达式为： 然后， 巡检机器人底盘位姿平 移向量th的表达式为： 其中， th表示底盘的位移 误差向量， P c表示校准状态下摄像机位姿， 表示拍摄状态下 的摄像机位姿， Pp表示云台位姿、 Ph表示校准状态下底盘位姿， Thp表示校准状态下巡检机器 人的位姿变换矩阵， 并且Thp＝(1， thp)和Tpc＝(1， tpc)， 其中thp和tpc表示巡检机器人在巡检 点的位置参数， RxRy表示标准状态下的正交矩阵， 分别对应于摄像机位姿Pc的俯仰角 θx和旋 转角θy， 表示拍摄状态下的摄像机在调节前的位资， Tc表示摄像机位姿Pc的误差， 为拍摄状态下的正交矩阵， 分别对应拍摄状态下位姿 的俯仰角θx和旋转角θy， RcxRcy表示摄像机对应预定拍摄画面的俯仰角度和旋转角度误差的正交矩阵， thp和tpc表示 巡检机器人在巡检点的位置参数， 表示拍摄状态下的底盘位姿， 表示拍摄状态下巡 检机器人 的位姿变换矩阵， 表示当前摄像机位姿的向量， 也就是当前摄像机位姿Pc 的俯仰角 θx和旋转角 θy的向量， tc＝(tcx， tcy， tcz)T是摄像机的位移误差向量， (tcx， tcy， tcz) 分别为变电站 三维模型中的以摄像机为坐标系， 其中摄像机所在位置的X轴、 Y轴和Z轴的坐 标参数。 7.如权利要求6所述的变电站巡检机器人位姿误差计算方法， 其特 征在于： 对巡检机器人进行重新定位， 获取巡检机器人在变电站三维模型地图中的坐标， 根据 底盘的位移误差向量th， 对所述巡检机器人位姿误差进行补偿， 而将巡检机器人导航到预 定巡检点。权　利　要　求　书 2/2 页 3 CN 115157243 A 3