水下机器人姿态估算方法、装置、电子设备及存储介质与流程

1.本发明属于水下计量技术领域，具体涉及到一种水下机器人姿态估算方法、装置、电子设备及存储介质。


背景技术：

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


技术实现要素：

4.本发明提供一种水下机器人姿态估算方法、装置、电子设备及存储介质，以解决现有的基于三轴陀螺的惯性导航系统获取的水下机器人姿态信息易受漂移影响的问题。
5.基于上述目的，本发明实施例提供了一种水下机器人姿态估算方法，包括：在参考坐标系内布置有多个包括声学信标和换能器的水下声学定位系统，获取所述声学信标对于在参考坐标系的第一位置信息；所述换能器安装在水下机器人上，通过所述换能器测量所述声学信标相对于所述换能器的第二位置信息，并根据所述第二位置信息获取所述声学信标相对水下机器人的第三位置信息；根据所述第一位置信息、所述第三位置信息获取所述参考坐标系向所述机器人本体坐标系变换的第一变换矩阵；根据所述第一变换矩阵以及所述水下机器人相对所述参考坐标系的欧拉角关系矩阵计算当前时刻所述水下机器人相对所述参考坐标系的姿态信息。
6.可选的，所述根据所述第二位置信息获取所述声学信标相对所述水下机器人的第三位置信息，包括：获取所述换能器在所述水下机器人上的安装角度和安装位置；根据所述第二位置信息以及所述换能器在水下机器人上的所述安装角度和所述安装位置获取所述声学信标相对所述水下机器人的第三位置信息。
7.可选的，所述根据所述第二位置信息以及所述换能器在水下机器人上的所述安装角度和所述安装位置获取所述声学信标相对所述水下机器人的第三位置信息，包括：根据所述安装角度和所述安装位置确定换能器坐标系到水下机器人坐标系的第二变换矩阵；将所述第二变换矩阵与所述第二位置信息的矩阵乘积与换能器坐标系原点在所述机器人坐标系内的第一位置偏差求和，获取所述声学信标相对所述水下机器人的所述第三位置信息。
8.可选的，所述根据所述第一位置信息、所述第三位置信息获取所述参考坐标系向所述机器人本体坐标系变换的第一变换矩阵，包括：获取所述第一位置信息、所述第三位置信息以及所述第一变换矩阵之间的相互关系式，所述相互关系式为第一变换矩阵与所述第三位置信息的矩阵乘积等于所述第一位置信息与机器人坐标系原点在所述参考坐标系内的第二位置偏差的差值；获取任意三个所述声学信标的位置坐标，并三个所述声学信标的位置坐标应用到所述相互关系式计算获取所述第一变换矩阵。
9.可选的，所述根据所述第一变换矩阵以及所述水下机器人相对所述参考坐标系的欧拉角关系矩阵计算当前时刻所述水下机器人相对所述参考坐标系的姿态信息，包括：根据所述欧拉角关系矩阵中各点与所述第一变换矩阵中各点的一一对应关系计算所述姿态信息中的各角度的三角函数值，并根据所述三角函数值分别计算所述姿态信息中的各角度，其中所述姿态信息包括航向角、俯仰角、滚转角。
10.可选的，所述根据所述欧拉角关系矩阵中各点与所述第一变换矩阵中各点的一一对应关系计算所述姿态信息中的各角度的三角函数值，并根据所述三角函数值分别计算所述姿态信息中的各角度，包括：根据所述第一变换矩阵中第一行第三列的值以及所述欧拉角关系矩阵确定所述俯仰角的正弦值，并根据所述俯仰角的正弦值计算可能的所述俯仰角；根据所述第一变换矩阵中各点的值、可能的所述俯仰角以及所述欧拉角关系矩阵获取所述航向角的正弦值和余弦值以及所述滚转角的正弦值和余弦值；根据所述航向角的正弦值和余弦值以及所述滚转角的正弦值和余弦值分别计算所述航向角和所述滚转角。
11.可选地，所述根据所述第一变换矩阵中各点的值、可能的所述俯仰角以及所述欧拉角关系矩阵获取所述航向角的正弦值和余弦值以及所述滚转角的正弦值和余弦值，包括：根据所述第一变换矩阵中第一行的第一列和第二列的值、可能的所述俯仰角以及所述欧拉角关系矩阵确定所述航向角的候选正弦值和候选余弦值；根据所述第一变换矩阵中第二行第三列和第三行第三列的值、可能的所述俯仰角以及所述欧拉角关系矩阵确定所述滚转角的候选正弦值和候选余弦值；根据所述第一变换矩阵中其他各点的值、所述航向角的候选正弦值和候选余弦值以及所述滚转角的候选正弦值和候选余弦值确定所述航向角的正弦值和余弦值以及所述滚转角的正弦值和余弦值。
12.基于同一发明构思，本发明实施例还提出了一种水下机器人姿态估算装置，包括：位置获取单元，用于获取声学信标对于在参考坐标系的第一位置信息，其中在参考坐标系内布置有多个包括所述声学信标和换能器的水下声学定位系统；位置转换单元，用于通过所述换能器测量所述声学信标相对于所述换能器的第二位置信息，并根据所述第二位置信息获取所述声学信标相对水下机器人的第三位置信息，其中所述换能器安装在水下机器人上；变换矩阵获取单元，用于根据所述第一位置信息、所述第三位置信息获取所述参考坐标系向所述机器人本体坐标系变换的第一变换矩阵；姿态计算单元，用于根据所述第一变换矩阵以及所述水下机器人相对所述参考坐标系的欧拉角关系矩阵计算当前时刻所述水下机器人相对所述参考坐标系的姿态信息。
13.基于同一发明构思，本发明实施例还提出了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现前述的方法。
14.基于同一发明构思，本发明实施例还提出了一种计算机存储介质，存储介质中存
储有至少一可执行指令，所述可执行指令使处理器执行前述的方法。
15.本发明的有益效果是：从上面所述可以看出，本发明实施例提供的一种水下机器人姿态估算方法、装置、电子设备及存储介质，方法包括：在参考坐标系内布置有多个包括声学信标和换能器的水下声学定位系统，获取所述声学信标对于在参考坐标系的第一位置信息；所述换能器安装在水下机器人上，通过所述换能器测量所述声学信标相对于所述换能器的第二位置信息，并根据所述第二位置信息获取所述声学信标相对水下机器人的第三位置信息；根据所述第一位置信息、所述第三位置信息获取所述参考坐标系向所述机器人本体坐标系变换的第一变换矩阵；根据所述第一变换矩阵以及所述水下机器人相对所述参考坐标系的欧拉角关系矩阵计算当前时刻所述水下机器人相对所述参考坐标系的姿态信息，通过将声学信标安装在不同的参考坐标系内，能够准确测量水下机器人相对不同参考坐标系的姿态信息。
附图说明
16.为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
17.图1为本发明实施例中的水下机器人姿态估算方法的流程示意图；
18.图2为本发明实施例中的水下声学定位系统的设置示意图；
19.图3为本发明实施例中的水下机器人姿态估算装置的结构示意图；
20.图4为本发明实施例中电子设备示意图。
具体实施方式
21.为使本公开的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本公开进一步详细说明。
22.需要说明的是，除非另外定义，本发明实施例使用的技术术语或者科学术语应当为本公开所属领域内具有一般技能的人士所理解的通常意义。本发明实施例中使用的“第一”、“第二”以及类似的词语并不表示任何顺序、数量或者重要性，而只是用来区分不同的组成部分。“包括”或者“包含”等类似的词语意指出现该词前面的元件或者物件涵盖出现在该词后面列举的元件或者物件及其等同，而不排除其他元件或者物件。“连接”或者“相连”等类似的词语并非限定于物理的或者机械的连接，而是可以包括电性的连接，不管是直接的还是间接的。“上”、“下”、“左”、“右”等仅用于表示相对位置关系，当被描述对象的绝对位置改变后，则该相对位置关系也可能相应地改变。
23.本发明实施例提供了一种水下机器人姿态估算方法。如附图1所示，水下机器人姿态估算方法包括：
24.步骤s11：获取声学信标对于在参考坐标系的第一位置信息，其中在所述参考坐标系内布置有多个包括所述声学信标和换能器的水下声学定位系统。
25.在本发明实施例中，在步骤s11之前，根据需求选择合适的参考坐标系。例如参考坐标系可以选择地球坐标系、水下平台或者水面船舶上等。为了方便叙述，本发明实施例中
选择地球坐标系作为参考坐标系。
26.在步骤s11中，在对应的参考坐标系内，布置若干个水下声学定位系统所需的声学信标，要求任何时刻都可以直接测量声学信标在参考坐标系的位置信息(xyz轴信息)，得到所述第一位置信息；或者可以将声学信标固定在参考坐标系内，事先测量好声学信标的第一位置信息。其中，水下声学定位系统优选为超短基线定位系统(ultra-short baseline acoustic positioning system，usbl)。例如，水下声学定位系统如图2所示，换能器设置在水下机器人上，3个声学信标设置在水平面以下不同位置，(x
usbl
，y
usbl
，z
usbl
)即为换能器对应的坐标系o
trans-x
transytransztrans
。为换能器相对水下机器人的姿态信息，为水下机器人相对参考坐标系的姿态信息，其中roll为滚转角，pitch为俯仰角，heading为航向角。
27.步骤s12：通过所述换能器测量所述声学信标相对于所述换能器的第二位置信息，并根据所述第二位置信息获取所述声学信标相对水下机器人的第三位置信息，其中，所述换能器安装在水下机器人上。
28.在本发明实施例中，水下声学定位系统所需的换能器安装在水下机器人上，在换能器上定义对应的坐标系o
trans-x
transytransztrans
。通过换能器可以连续不断地测量各个声学信标相对于换能器的第二位置信息其中i表示第i个声学信标。
29.然后获取所述换能器在所述水下机器人上的安装角度和安装位置。在水下机器人上定义机器人坐标系o
rov-x
rovyrovzrov
，再根据所述第二位置信息以及所述换能器在水下机器人上的所述安装角度和所述安装位置获取所述声学信标相对所述水下机器人的第三位置信息。可选地，根据所述安装角度和所述安装位置确定换能器坐标系到水下机器人坐标系的第二变换矩阵；将所述第二变换矩阵与所述第二位置信息的矩阵乘积与换能器坐标系原点在所述机器人坐标系内的第一位置偏差求和，获取所述声学信标相对所述水下机器人的所述第三位置信息：
[0030][0031]
其中，表示声学信标相对水下机器人的所述第三位置信息，为换能器坐标系到机器人坐标系的第二变换矩阵，与换能器在水下机器人上的安装偏差角有关。表示换能器坐标系原点在机器人坐标系内的位置，即换能器安装的第一位置偏差。
[0032]
步骤s13：根据所述第一位置信息、所述第三位置信息获取所述参考坐标系向所述机器人本体坐标系变换的第一变换矩阵。
[0033]
在本发明实施例中，将声学信标相对于机器人坐标系内的第三位置信息通过坐标变换，转换成声学信标相对于参考坐标系的第一位置信息，由于此处选择地球坐标系作为
参考坐标系，对应的计算关系如下：
[0034][0035]
其中，表示换能器坐标系原点在参考坐标系内的第二位置偏差，亦即换能器坐标系原点在参考坐标系内的位置坐标，表示参考坐标系向机器人本体坐标系变换的第一变换矩阵。
[0036]
可选地，获取所述第一位置信息、所述第三位置信息以及所述第一变换矩阵之间的相互关系式，所述相互关系式为第一变换矩阵与所述第三位置信息的矩阵乘积等于所述第一位置信息与机器人坐标系原点在所述参考坐标系内的第二位置偏差的差值；获取任意三个所述声学信标的位置坐标，并三个所述声学信标的位置坐标应用到所述相互关系式计算获取所述第一变换矩阵。具体地，根据前述的声学信标相对于机器人坐标系内的第三位置信息通过坐标变换转换成声学信标相对于参考坐标系的第一位置信息的计算关系，通过推导得到第一位置信息、第三位置信息以及第一变换矩阵之间的相互关系式如下：
[0037][0038]
其中，
[0039]
任意选取其中的三个声学信标的位置坐标，应用到相互关系式中可以得到如下计算公式：
[0040][0041]
根据以上计算公式计算得到第一变换矩阵中每行每列中各点的值。
[0042]
步骤s14：根据所述第一变换矩阵以及所述水下机器人相对所述参考坐标系的欧拉角关系矩阵计算当前时刻所述水下机器人相对所述参考坐标系的姿态信息。
[0043]
在本发明实施例中，水下机器人相对参考坐标系的欧拉角关系矩阵中各点与第一变换矩阵中各点呈一一对应关系：
[0044][0045]
其中，θ为水下机器人的俯仰角，ψ为水下机器人的航向角，φ为水下机器人的滚转角。
[0046]
在步骤s14中，可选地，根据所述欧拉角关系矩阵中各点与所述第一变换矩阵中各点的一一对应关系计算所述姿态信息中的各角度的三角函数值，并根据所述三角函数值分别计算所述姿态信息中的各角度，其中所述姿态信息包括航向角、俯仰角、滚转角。
[0047]
具体的，首先根据所述第一变换矩阵中第一行第三列的值以及所述欧拉角关系矩阵确定所述俯仰角的正弦值，并根据所述俯仰角的正弦值计算可能的所述俯仰角。根据第一变换矩阵中第一行第三列的值t
13
以及欧拉角关系矩阵确定俯仰角的正弦值sinθ，进而俯仰角的正弦值sinθ计算的俯仰角可能为θ＝arcsin(-t
13
)或θ＝π-arcsin(-t
13
)。
[0048]
然后根据所述第一变换矩阵中各点的值、可能的所述俯仰角以及所述欧拉角关系矩阵获取所述航向角的正弦值和余弦值以及所述滚转角的正弦值和余弦值；再根据所述航向角的正弦值和余弦值以及所述滚转角的正弦值和余弦值分别计算所述航向角和所述滚转角。
[0049]
在本发明实施例中，根据所述第一变换矩阵中第一行的第一列和第二列的值、可能的所述俯仰角以及所述欧拉角关系矩阵确定所述航向角的候选正弦值和候选余弦值。即根据第一变换矩阵中t
11
、t
12
和俯仰角θ计算cosψ和sinψ。根据所述第一变换矩阵中第二行
第三列和第三行第三列的值、可能的所述俯仰角以及所述欧拉角关系矩阵确定所述滚转角的候选正弦值和候选余弦值。即根据第一变换矩阵中t
23
、t
33
和俯仰角θ计算cosφ和sinφ。根据所述第一变换矩阵中其他各点的值、所述航向角的候选正弦值和候选余弦值以及所述滚转角的候选正弦值和候选余弦值确定所述航向角的正弦值和余弦值以及所述滚转角的正弦值和余弦值。可以分别将两个不同的俯仰角θ计算结果和对应的cosψ、sinψ、cosφ和sinφ计算对应的t
21
、t
22
、t
31
和t
32
，并验证计算结果与第一变换矩阵中的t
21
、t
22
、t
31
和t
32
是否一致，选择与第一变换矩阵中的t
21
、t
22
、t
31
和t
32
一致的计算结果对应的cosψ、sinψ、cosφ和sinφ，进而根据cosψ、sinψ、cosφ和sinφ分别计算对应的水下机器人的航向角ψ和滚转角φ。
[0050]
至此，计算得到的俯仰角θ、航向角ψ和滚转角φ共同构成水下机器人相对参考坐标系的姿态信息。
[0051]
在本发明实施例中，应用不同坐标系变换关系进行基于声学定位系统的水下机器人姿态角估算，由于使用了固定的地球坐标系作为参考坐标系，不存在时间漂移的问题，可用于单独给水下机器人进行姿态测量，也可以用于校准传统惯性导航姿态测量系统。另外，通过改变参考坐标系，比如选择在船舶或者水下工作平台上定义参考坐标系，能够非常便捷地获得水下机器人相对参考坐标系的姿态角信息，进而方便完成某些需要协同操作的作业任务。
[0052]
本发明实施例的水下机器人姿态估算方法通过获取声学信标对于在参考坐标系的第一位置信息，其中在所述参考坐标系内布置有多个包括所述声学信标和换能器的水下声学定位系统；通过所述换能器测量所述声学信标相对于所述换能器的第二位置信息，并根据所述第二位置信息获取所述声学信标相对水下机器人的第三位置信息，其中所述换能器安装在水下机器人上；根据所述第一位置信息、所述第三位置信息获取所述参考坐标系向所述机器人本体坐标系变换的第一变换矩阵；根据所述第一变换矩阵以及所述水下机器人相对所述参考坐标系的欧拉角关系矩阵计算当前时刻所述水下机器人相对所述参考坐标系的姿态信息，通过将声学信标安装在不同的参考坐标系内，能够准确测量水下机器人相对不同参考坐标系的姿态信息。
[0053]
上述对本发明特定实施例进行了描述。在一些情况下，在本发明实施例中记载的动作或步骤可以按照不同于实施例中的顺序来执行并且仍然可以实现期望的结果。另外，在附图中描绘的过程不一定要求示出的特定顺序或者连续顺序才能实现期望的结果。在某些实施方式中，多任务处理和并行处理也是可以的或者可能是有利的。
[0054]
基于同一个构思，本发明实施例还提供了一种水下机器人姿态估算装置。应用于的自动驾驶装置或辅助驾驶装置。附图3所示，水下机器人姿态估算装置包括：位置获取单元、位置转换单元、变换矩阵获取单元以及姿态计算单元。其中，
[0055]
位置获取单元，用于获取声学信标对于在参考坐标系的第一位置信息，其中在参考坐标系内布置有多个包括所述声学信标和换能器的水下声学定位系统；
[0056]
位置转换单元，用于通过所述换能器测量所述声学信标相对于所述换能器的第二位置信息，并根据所述第二位置信息获取所述声学信标相对水下机器人的第三位置信息，其中所述换能器安装在水下机器人上；
[0057]
变换矩阵获取单元，用于根据所述第一位置信息、所述第三位置信息获取所述参
考坐标系向所述机器人本体坐标系变换的第一变换矩阵；
[0058]
姿态计算单元，用于根据所述第一变换矩阵以及所述水下机器人相对所述参考坐标系的欧拉角关系矩阵计算当前时刻所述水下机器人相对所述参考坐标系的姿态信息。
[0059]
为了描述的方便，描述以上装置时以功能分为各种模块分别描述。当然，在实施本发明实施例时可以把各模块的功能在同一个或多个软件和/或硬件中实现。
[0060]
上述实施例的装置应用于前述实施例中相应的方法，并且具有相应的方法实施例的有益效果，在此不再赘述。
[0061]
基于同一发明构思，本发明实施例还提供了一种电子设备，该电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如上任意一实施例所述的方法。
[0062]
本发明实施例提供了一种非易失性计算机存储介质，所述计算机存储介质存储有至少一可执行指令，该计算机可执行指令可执行如上任意一实施例中所述的方法。
[0063]
图4示出了本实施例所提供的一种更为具体的电子设备硬件结构示意图，该设备可以包括：处理器401、存储器402、输入/输出接口403、通信接口404和总线405。其中处理器401、存储器402、输入/输出接口403和通信接口404通过总线405实现彼此之间在设备内部的通信连接。
[0064]
处理器401可以采用通用的cpu(central processing unit，中央处理器)、微处理器、应用专用集成电路(application specific integrated circuit，asic)、或者一个或多个集成电路等方式实现，用于执行相关程序，以实现本发明方法实施例所提供的技术方案。
[0065]
存储器402可以采用rom(read only memory，只读存储器)、ram(random accessmemory，随机存取存储器)、静态存储设备，动态存储设备等形式实现。存储器402可以存储操作系统和其他应用程序，在通过软件或者固件来实现本发明方法实施例所提供的技术方案时，相关的程序代码保存在存储器402中，并由处理器401来调用执行。
[0066]
输入/输出接口403用于连接输入/输出模块，以实现信息输入及输出。输入输出/模块可以作为组件配置在设备中(图中未示出)，也可以外接于设备以提供相应功能。其中输入设备可以包括键盘、鼠标、触摸屏、麦克风、各类传感器等，输出设备可以包括显示器、扬声器、振动器、指示灯等。
[0067]
通信接口404用于连接通信模块(图中未示出)，以实现本设备与其他设备的通信交互。其中通信模块可以通过有线方式(例如usb、网线等)实现通信，也可以通过无线方式(例如移动网络、wifi、蓝牙等)实现通信。
[0068]
总线405包括一通路，在设备的各个组件(例如处理器401、存储器402、输入/输出接口403和通信接口404)之间传输信息。
[0069]
需要说明的是，尽管上述设备仅示出了处理器401、存储器402、输入/输出接口403、通信接口404以及总线405，但是在具体实施过程中，该设备还可以包括实现正常运行所必需的其他组件。此外，本领域的技术人员可以理解的是，上述设备中也可以仅包含实现本发明实施例方案所必需的组件，而不必包含图中所示的全部组件。
[0070]
所属领域的普通技术人员应当理解：以上任何实施例的讨论仅为示例性的，并非旨在暗示本技术的范围被限于这些例子；在本技术的思路下，以上实施例或者不同实施例
中的技术特征之间也可以进行组合，步骤可以以任意顺序实现，并存在如上所述的本技术的不同方面的许多其它变化，为了简明它们没有在细节中提供。
[0071]
本技术旨在涵盖落入本发明实施例的宽泛范围之内的所有这样的替换、修改和变型。因此，凡在本发明实施例的精神和原则之内，所做的任何省略、修改、等同替换、改进等，均应包含在本技术的保护范围之内。