一种姿态角耦合误差补偿方法、装置及电子设备与流程
本发明涉及导航定位领域，特别是指一种姿态角耦合误差补偿方法、系统及电子设备。
背景技术：
目前，无人机等导航系统在平行飞行的过程中，往往会因为一些原因而产生小范围的姿态角的变化，例如，机体在飞行的过程中又可能会偏离理想水平面而导致机体的横滚角、俯仰角以及航向角不为零。在无人机等导航系统的机顶上往往设置有激光雷达，以采集环境特征信息，若无人机等导航系统偏离理想水平面，此时环境特征信息将会偏移到其他平面，采用该环境特征信息进行环境特征匹配并定位得到的结果存在非常大的误差。
技术实现要素：
有鉴于此，本发明的目的在于提出一种姿态角耦合误差补偿方法、装置及电子设备，用于解决定位结果所产生的误差，提高定位精度。
基于上述目的，本发明提供了一种姿态角耦合误差补偿方法，包括：
获取机体的环境特征信息；
根据所述环境特征信息，确定所述机体所在的实际二维平面；
根据所述实际二维平面，建立机体坐标系；
通过导航系统测量所述机体的横滚角和俯仰角；
将所述机体坐标系通过坐标系转换转换为中间坐标系，以调节所述横滚角和所述俯仰角为零。
在本发明其他的一些实施方式中，所述根据所述实际二维平面，建立机体坐标系包括：
在所述实际二维平面内确定一点ob，在所述实际二维平面内建立obxb轴和obyb轴，并建立obzb轴垂直于所述实际二维平面。
在本发明其他的一些实施方式中，所述中间坐标系包括：ocxc轴、ocyc轴、oczc轴；所述ocxc轴和所述ocyc轴在理想水平面上；所述oczc轴垂直于所述理想水平面。
在本发明其他的一些实施方式中，所述将所述机体坐标系通过坐标系转换转换为中间坐标系包括：所述ocxc轴为所述obxb轴在所述理想水平面上的投影，所述ocyc轴为所述obyb轴在所述理想水平面上的投影。
在本发明其他的一些实施方式中，所述obxb轴垂直于所述obyb轴，所述ocxc轴垂直于所述ocyc轴。
在本发明其他的一些实施方式中，所述将所述机体坐标系通过坐标系转换转换为中间坐标系，以调节所述横滚角和所述俯仰角为零包括：
所述中间坐标系ocxcyczc转换为所述机体坐标系obxbybzb的坐标转换矩阵为:
转置得到所述机体坐标系obxbybzb转换为所述中间坐标系ocxcyczc的坐标转换矩阵
其中，γ为所述横滚角，θ为所述俯仰角。
基于同一发明构思，本发明还提供了一种姿态角耦合误差补偿装置，包括：
获取模块，用于获取机体的环境特征信息；
确定模块，用于根据所述环境特征信息，确定所述机体所在的实际二维平面；
建立模块，用于根据所述实际二维平面，建立机体坐标系；
测量模块，用于通过导航系统测量所述机体的横滚角和俯仰角；
修正模块，用于将所述机体坐标系通过坐标系转换转换为中间坐标系，以调节所述横滚角和所述俯仰角为零。
基于同一发明构思，本发明还提供了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如上述任意一项所述的方法。
从上面所述可以看出，本发明提供的一种姿态角耦合误差补偿方法、装置及电子设备，通过坐标转换的方式，能够较好地补偿无人机等设备在飞行过程中所偏离水平面产生的误差。本发明所提供的方法、装置以及电子设备，大大消除了无人机在室内定位时姿态角的影响，将无人机等设备在有姿态角时检测到的环境特征补偿到水平面上，大幅度提高了定位精度，为无人机等设备在航行的过程中使用更为高效、准确。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明一种姿态角耦合误差补偿方法实施例的流程示意图；
图2为本发明一种姿态角耦合误差补偿方法实施例的坐标转换示意图；
图3为本发明一种姿态角耦合误差补偿方法实施例的横滚角转换示意图；
图4为本发明一种姿态角耦合误差补偿方法实施例的俯仰角转换示意图；
图5为本发明一种姿态角耦合误差补偿方法实施例的航向角转换示意图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明进一步详细说明。
需要说明的是，本发明实施例中所有使用“第一”和“第二”的表述均是为了区分两个相同名称非相同的实体或者非相同的参量，可见“第一”“第二”仅为了表述的方便，不应理解为对本发明实施例的限定，后续实施例对此不再一一说明。
无人机等设备在进行航行的过程中，可能因为某些环境因素或者内部因素发生位置的偏转，很容易导致小型无人机在平行飞行过程中会有小范围内的姿态角变化。在本发明中，主要是通过坐标转换的方式消除无人机在平行飞行过程中所产生的横滚角和俯仰角，使得横滚角和俯仰角校正为零，从而使得在不改变无人机航向的基础上，使得无人机能够在理想的水平面上运动。
在本发明的一个实施例中，一种姿态角耦合误差补偿方法，参考图1，包括：
步骤101：获取机体的环境特征信息；
在本实施例中，通过激光雷达系统来获取机体的环境特征信息。主要是通过采集外部环境信息。具体地，在室内结构化使用环境中，由于墙壁、桌椅、书柜等物品一般是垂直于水平面放置的。因此，假设所有障碍物的表面都是由垂直于地面的若干平面构成。此时，采集的环境特征信息包括：以周围垂直于地面的平面为基准，墙角角点等可以用来确定位置关系的点。
激光雷达测量的是二维平面环境信息，小型无人机不同于地面行走的机器人，观测的是同一平面的二维环境信息。小型无人机在平行飞行过程中会有小范围内的姿态角变化，此时固定于无人机顶的激光雷达测量的环境特征信息将会偏移到其它平面，采用该环境特征信息进行环境特征匹配并定位得到的结果存在非常大的误差。因此，需要进行姿态补偿误差将环境特征信息补偿到同一平面。
步骤102：根据环境特征信息，确定机体所在的实际二维平面；
具体地，在本实施例中，根据环境特征信息中的可用来确定位置关系的点，由点确定线段，通过线段来确定机体所在的实际二维平面，即由点及线及面的来确定实际二维平面。
在本发明其他的一些实施例中，还可通过其他方式根据环境特征信息来确定机体所在的实际二维平面。
步骤103：根据所述实际二维平面，建立机体坐标系；
其中，根据所述实际二维平面，建立机体坐标系包括：
在实际二维平面内确定一点ob，在实际二维平面内建立obxb轴和obyb轴，并建立obzb轴垂直于实际二维平面。
具体地，机体坐标系是根据机体所在的实际二维平面所构建的一个坐标系。点ob为机体的中心，以机体的中心点ob为原点，建立obxb轴和obyb轴。在本实施例中，obxb轴垂直于obyb轴，然后通过所述原点，建立obzb轴垂直于实际二维平面。在本发明其他的一些实施例中，obxb轴也可不必然垂直于obyb轴。其中，建立的机体坐标系如图2所示，面pb即为实际二维平面，obxbybzb即为机体坐标系。
进一步地来说，根据实际二维平面建立机体坐标系，是为了将机体的实际位置进行坐标化的处理，使得之后的处理更加简化而便于操作。
步骤104：通过导航系统测量所述机体的横滚角和俯仰角；
具体地，在本发明的一些实施例中，包括激光雷达系统和导航系统，其中，激光雷达系统的作用由前可知，在此不再赘述。在本发明中，一般还设置有导航系统，其中，导航系统一般用于实时测量所述机体的横滚角和俯仰角，以实时监测所述机体是否偏离理想水平面以及具体偏离多少。
在本实施例中，参考图3和图4，表示的分别是横滚角γ和俯仰角θ。其中，图5表示的为航向角ψ，在本发明所提供的技术方案中，并不改变机体的航向角ψ。因为，在本发明中所提供的方法只是为了补偿其偏离水平位置的误差，对于无人机的航向，在本发明中，并不会修改机体的航向，还是通过原来的航线完成整个航线过程。
步骤105：将机体坐标系通过坐标系转换转换为中间坐标系，以调节所述横滚角和所述俯仰角为零。
其中，参考图2，理想水平面为面pc，中间坐标系ocxcyczc包括：ocxc轴、ocyc轴、oczc轴；ocxc轴和ocyc轴在理想水平面上；oczc轴垂直于理想水平面。
具体地，点oc即为点ob，以点oc为原点，将机体坐标系obxbybzb通过坐标系转换转换为中间坐标系ocxcyczc包括：ocxc轴为obxb轴在理想水平面上的投影，ocyc轴为obyb轴在理想水平面上的投影，过原点oc建立oczc轴垂直于理想水平面。
激光雷达在扫描点中提取到特征点是在pb平面内的ab(xb,yb,0)，经过一系列坐标转换得到该特征点在机体坐标系下pc平面上的ac(xc,yc,zc)，也就是补偿后的的平面内特征点位置，也是进行环境特征提取和环境特征匹配所需的水平探测坐标，求得该点即可与前一帧扫描点簇中的特征进行特征匹配，实现全局地图的更新以及无人机的自定位。因此，需要建立坐标转换矩阵，将特征补偿到同一平面上即可进一步的特征匹配。
其中，为计算该坐标值ac(xc,yc,zc)，设无人机相对中间坐标系ocxcyczc的俯仰角、横滚角、航向角分别用θ,γ,ψ表示，θ,γ,ψ表示从ocxcyczc到机体系oxbybzb的一种转动，参考图5、图4以及图3，如下式(1)所示：
由此可知，xcyczc在ozc轴不动的情况下偏转ψ角度到x′cy′cz′c，x′cy′cz′c在ox′c轴不动的情况下偏转θ角度到x″cy″cz″c，x″cy″cz″c在oy″b轴不动的情况下偏转γ角度到xbybzb，且在本实施例中，ψ＝0，即并不改变航向角的大小，前述已述，在此不再赘述。
具体地，将机体坐标系通过坐标系转换转换为中间坐标系，以调节横滚角和俯仰角为零的具体过程包括：
中间坐标系ocxcyczc转换为机体坐标系obxbybzb的坐标转换矩阵为:
转置得到机体坐标系obxbybzb转换为中间坐标系ocxcyczc的坐标转换矩阵
其中，γ为横滚角，θ为俯仰角。
具体地，在本实施例中，无人机在飞行过程中会存在一定的横滚角、俯仰角，这些都会影响到无人机最终的定位结果。因此，通过误差补偿方式，推算机体坐标系与中间坐标系的角度关系，获得坐标系转换矩阵
因而，将机体坐标系obxbybzb转换为中间坐标系ocxcyczc的过程，实际上是将obxb轴和obyb轴转换到理想水平面的过程，而在此过程中，消除了消除无人机在平行飞行过程中所产生的横滚角和俯仰角，使得横滚角和俯仰角校正为零，从而使得在不改变无人机航向的基础上，使得无人机能够在理想的水平面上运动。
基于同一发明构思，本发明还提供了一种姿态角耦合误差补偿装置，包括：
获取模块，用于获取机体的环境特征信息；
确定模块，用于根据环境特征信息，确定机体所在的实际二维平面；
建立模块，用于根据实际二维平面，建立机体坐标系；
测量模块，用于通过导航系统测量所述机体的横滚角和俯仰角；
修正模块，用于将机体坐标系通过坐标系转换转换为中间坐标系，以调节横滚角和俯仰角为零。
基于同一发明构思，本发明还提供了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如上述任意一项所述的方法。
上述实施例的装置用于实现前述实施例中相应的方法，并且具有相应的方法实施例的有益效果，在此不再赘述。
所属领域的普通技术人员应当理解：以上任何实施例的讨论仅为示例性的，并非旨在暗示本公开的范围(包括权利要求)被限于这些例子；在本发明的思路下，以上实施例或者不同实施例中的技术特征之间也可以进行组合，步骤可以以任意顺序实现，并存在如上所述的本发明的不同方面的许多其它变化，为了简明它们没有在细节中提供。
另外，为简化说明和讨论，并且为了不会使本发明难以理解，在所提供的附图中可以示出或可以不示出与集成电路(ic)芯片和其它部件的公知的电源/接地连接。此外，可以以框图的形式示出装置，以便避免使本发明难以理解，并且这也考虑了以下事实，即关于这些框图装置的实施方式的细节是高度取决于将要实施本发明的平台的(即，这些细节应当完全处于本领域技术人员的理解范围内)。在阐述了具体细节以描述本发明的示例性实施例的情况下，对本领域技术人员来说显而易见的是，可以在没有这些具体细节的情况下或者这些具体细节有变化的情况下实施本发明。因此，这些描述应被认为是说明性的而不是限制性的。
尽管已经结合了本发明的具体实施例对本发明进行了描述，但是根据前面的描述，这些实施例的很多替换、修改和变型对本领域普通技术人员来说将是显而易见的。例如，其它存储器架构可以使用所讨论的实施例。
本发明的实施例旨在涵盖落入所附权利要求的宽泛范围之内的所有这样的替换、修改和变型。因此，凡在本发明的精神和原则之内，所做的任何省略、修改、等同替换、改进等，均应包含在本发明的保护范围之内。
- 还没有人留言评论。精彩留言会获得点赞！