水下机器人姿态估算方法、装置、电子设备及存储介质制造方法及图纸

本发明专利技术提供一种水下机器人姿态估算方法、装置、电子设备及存储介质，方法包括：获取声学信标对于在参考坐标系的第一位置信息，参考坐标系内布置有多个包括声学信标和换能器的水下声学定位系统；通过换能器测量声学信标相对于换能器的第二位置信息，进而获取声学信标相对水下机器人的第三位置信息，其中换能器安装在水下机器人上；根据第一位置信息、第三位置信息获取参考坐标系向机器人本体坐标系变换的第一变换矩阵；根据第一变换矩阵以及水下机器人相对参考坐标系的欧拉角关系矩阵计算当前时刻水下机器人相对参考坐标系的姿态信息。本发明专利技术通过将声学信标安装在不同的参考坐标系内，能够准确测量水下机器人相对不同参考坐标系的姿态信息。标系的姿态信息。标系的姿态信息。

Attitude estimation method, device, electronic equipment and storage medium of underwater robot

【技术实现步骤摘要】
水下机器人姿态估算方法、装置、电子设备及存储介质


[0001]本专利技术属于水下计量
，具体涉及到一种水下机器人姿态估算方法、装置、电子设备及存储介质。

技术介绍

[0002]水下机器人在水下飞行或者作业时，需要不断地上传自身状态信息，包括位置信息和姿态信息，以便于操作人员做出及时响应。并且在自动控制系统操作时，水下机器人也需要不断将自身状态信息发送给控制系统作为状态反馈，以实现精准的自动控制功能。
[0003]为了获取水下机器人姿态信息，通常需要在机器人上安装三轴高精度陀螺传感器，即通常所说的惯性导航系统。基于三轴陀螺的惯性导航系统具有精度高、稳定性好等优点，缺点则是价格昂贵、长时间运行会受漂移等因素影响。另外，惯性导航系统通常计算出来的姿态信息是相对于地球坐标系，但在某些特殊场合，比如水下自动对接、对象物跟踪和相对位置保持，地球坐标系的姿态角信息不能够直接用于自动操作水下机器人完成对应的作业任务。

技术实现思路

[0004]本专利技术提供一种水下机器人姿态估算方法、装置、电子设备及存储介质，以解决现有的基于三轴陀螺的惯性导航系统获取的水下机器人姿态信息易受漂移影响的问题。
[0005]基于上述目的，本专利技术实施例提供了一种水下机器人姿态估算方法，包括：在参考坐标系内布置有多个包括声学信标和换能器的水下声学定位系统，获取所述声学信标对于在参考坐标系的第一位置信息；所述换能器安装在水下机器人上，通过所述换能器测量所述声学信标相对于所述换能器的第二位置信息，并根据所述第二位置信息获取所述声学信标相对水下机器人的第三位置信息；根据所述第一位置信息、所述第三位置信息获取所述参考坐标系向所述机器人本体坐标系变换的第一变换矩阵；根据所述第一变换矩阵以及所述水下机器人相对所述参考坐标系的欧拉角关系矩阵计算当前时刻所述水下机器人相对所述参考坐标系的姿态信息。
[0006]可选的，所述根据所述第二位置信息获取所述声学信标相对所述水下机器人的第三位置信息，包括：获取所述换能器在所述水下机器人上的安装角度和安装位置；根据所述第二位置信息以及所述换能器在水下机器人上的所述安装角度和所述安装位置获取所述声学信标相对所述水下机器人的第三位置信息。
[0007]可选的，所述根据所述第二位置信息以及所述换能器在水下机器人上的所述安装角度和所述安装位置获取所述声学信标相对所述水下机器人的第三位置信息，包括：根据所述安装角度和所述安装位置确定换能器坐标系到水下机器人坐标系的第二变换矩阵；将所述第二变换矩阵与所述第二位置信息的矩阵乘积与换能器坐标系原点在所述机器人坐标系内的第一位置偏差求和，获取所述声学信标相对所述水下机器人的所述第三位置信息。
[0008]可选的，所述根据所述第一位置信息、所述第三位置信息获取所述参考坐标系向所述机器人本体坐标系变换的第一变换矩阵，包括：获取所述第一位置信息、所述第三位置信息以及所述第一变换矩阵之间的相互关系式，所述相互关系式为第一变换矩阵与所述第三位置信息的矩阵乘积等于所述第一位置信息与机器人坐标系原点在所述参考坐标系内的第二位置偏差的差值；获取任意三个所述声学信标的位置坐标，并三个所述声学信标的位置坐标应用到所述相互关系式计算获取所述第一变换矩阵。
[0009]可选的，所述根据所述第一变换矩阵以及所述水下机器人相对所述参考坐标系的欧拉角关系矩阵计算当前时刻所述水下机器人相对所述参考坐标系的姿态信息，包括：根据所述欧拉角关系矩阵中各点与所述第一变换矩阵中各点的一一对应关系计算所述姿态信息中的各角度的三角函数值，并根据所述三角函数值分别计算所述姿态信息中的各角度，其中所述姿态信息包括航向角、俯仰角、滚转角。
[0010]可选的，所述根据所述欧拉角关系矩阵中各点与所述第一变换矩阵中各点的一一对应关系计算所述姿态信息中的各角度的三角函数值，并根据所述三角函数值分别计算所述姿态信息中的各角度，包括：根据所述第一变换矩阵中第一行第三列的值以及所述欧拉角关系矩阵确定所述俯仰角的正弦值，并根据所述俯仰角的正弦值计算可能的所述俯仰角；根据所述第一变换矩阵中各点的值、可能的所述俯仰角以及所述欧拉角关系矩阵获取所述航向角的正弦值和余弦值以及所述滚转角的正弦值和余弦值；根据所述航向角的正弦值和余弦值以及所述滚转角的正弦值和余弦值分别计算所述航向角和所述滚转角。
[0011]可选地，所述根据所述第一变换矩阵中各点的值、可能的所述俯仰角以及所述欧拉角关系矩阵获取所述航向角的正弦值和余弦值以及所述滚转角的正弦值和余弦值，包括：根据所述第一变换矩阵中第一行的第一列和第二列的值、可能的所述俯仰角以及所述欧拉角关系矩阵确定所述航向角的候选正弦值和候选余弦值；根据所述第一变换矩阵中第二行第三列和第三行第三列的值、可能的所述俯仰角以及所述欧拉角关系矩阵确定所述滚转角的候选正弦值和候选余弦值；根据所述第一变换矩阵中其他各点的值、所述航向角的候选正弦值和候选余弦值以及所述滚转角的候选正弦值和候选余弦值确定所述航向角的正弦值和余弦值以及所述滚转角的正弦值和余弦值。
[0012]基于同一专利技术构思，本专利技术实施例还提出了一种水下机器人姿态估算装置，包括：位置获取单元，用于获取声学信标对于在参考坐标系的第一位置信息，其中在参考坐标系内布置有多个包括所述声学信标和换能器的水下声学定位系统；位置转换单元，用于通过所述换能器测量所述声学信标相对于所述换能器的第二位置信息，并根据所述第二位置信息获取所述声学信标相对水下机器人的第三位置信息，其中所述换能器安装在水下机器人上；变换矩阵获取单元，用于根据所述第一位置信息、所述第三位置信息获取所述参考坐标系向所述机器人本体坐标系变换的第一变换矩阵；姿态计算单元，用于根据所述第一变换矩阵以及所述水下机器人相对所述参考坐标系的欧拉角关系矩阵计算当前时刻所述水下机器人相对所述参考坐标系的姿态信息。
[0013]基于同一专利技术构思，本专利技术实施例还提出了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现前述的方法。
[0014]基于同一专利技术构思，本专利技术实施例还提出了一种计算机存储介质，存储介质中存
储有至少一可执行指令，所述可执行指令使处理器执行前述的方法。
[0015]本专利技术的有益效果是：从上面所述可以看出，本专利技术实施例提供的一种水下机器人姿态估算方法、装置、电子设备及存储介质，方法包括：在参考坐标系内布置有多个包括声学信标和换能器的水下声学定位系统，获取所述声学信标对于在参考坐标系的第一位置信息；所述换能器安装在水下机器人上，通过所述换能器测量所述声学信标相对于所述换能器的第二位置信息，并根据所述第二位置信息获取所述声学信标相对水下机器人的第三位置信息；根据所述第一位置信息、所述第三位置信息获取所述参考坐标系向所述机器人本体坐标系变换的第一变换矩阵；根据所述第一变换矩阵以及所述水下机器人相对所述参考坐标系的欧拉角关系矩阵计算当前时刻所述水下机器人相对所述参考坐标系的姿态信息，通过将声学信标安装在不同的本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种水下机器人姿态估算方法，其特征是，所述方法包括：获取声学信标对于在参考坐标系的第一位置信息，其中在所述参考坐标系内布置有多个包括所述声学信标和换能器的水下声学定位系统；通过所述换能器测量所述声学信标相对于所述换能器的第二位置信息，并根据所述第二位置信息获取所述声学信标相对水下机器人的第三位置信息，其中所述换能器安装在水下机器人上；根据所述第一位置信息、所述第三位置信息获取所述参考坐标系向所述机器人本体坐标系变换的第一变换矩阵；根据所述第一变换矩阵以及所述水下机器人相对所述参考坐标系的欧拉角关系矩阵计算当前时刻所述水下机器人相对所述参考坐标系的姿态信息。2.如权利要求1所述的方法，其特征是，所述根据所述第二位置信息获取所述声学信标相对所述水下机器人的第三位置信息，包括：获取所述换能器在所述水下机器人上的安装角度和安装位置；根据所述第二位置信息以及所述换能器在水下机器人上的所述安装角度和所述安装位置获取所述声学信标相对所述水下机器人的第三位置信息。3.如权利要求2所述的方法，其特征是，所述根据所述第二位置信息以及所述换能器在水下机器人上的所述安装角度和所述安装位置获取所述声学信标相对所述水下机器人的第三位置信息，包括：根据所述安装角度和所述安装位置确定换能器坐标系到水下机器人坐标系的第二变换矩阵；将所述第二变换矩阵与所述第二位置信息的矩阵乘积与换能器坐标系原点在所述机器人坐标系内的第一位置偏差求和，获取所述声学信标相对所述水下机器人的所述第三位置信息。4.如权利要求1所述的方法，其特征是，所述根据所述第一位置信息、所述第三位置信息获取所述参考坐标系向所述机器人本体坐标系变换的第一变换矩阵，包括：获取所述第一位置信息、所述第三位置信息以及所述第一变换矩阵之间的相互关系式，所述相互关系式为第一变换矩阵与所述第三位置信息的矩阵乘积等于所述第一位置信息与机器人坐标系原点在所述参考坐标系内的第二位置偏差的差值；获取任意三个所述声学信标的位置坐标，并三个所述声学信标的位置坐标应用到所述相互关系式计算获取所述第一变换矩阵。5.如权利要求1所述的方法，其特征是，所述根据所述第一变换矩阵以及所述水下机器人相对所述参考坐标系的欧拉角关系矩阵计算当前时刻所述水下机器人相对所述参考坐标系的姿态信息，包括：根据所述欧拉角关系矩阵中各点与所述第一变换矩阵中各点的一一对应关系计算所述姿态信息中的各角度的三角函数值，并根据所述三角函数值分别计算所述姿态信息中的各角度，其中所述姿态信息包括航向角、俯仰角、滚转角。6.如权利要求5所述的方法，其特征...

【专利技术属性】
技术研发人员：朱迎谷，张定华，尚敬，严允，涂绍平，卢倪斌，胡斌炜，朱建波，宋俊辉，王勇，徐蕾，
申请(专利权)人：上海中车艾森迪海洋装备有限公司，
类型：发明
国别省市：