一种标定方法、装置及飞行器与流程

本发明涉及机器人视觉技术领域，尤其涉及一种标定方法、装置及飞行器。

背景技术：

随着无人飞行器技术的发展，无人飞行器被广泛应用于军事侦察、灾区救援、地址勘探等领域。

相机和imu(inertialmeasurementunit，惯性测量模块)是飞行器中的重要元件，由于相机在快速运动、光照改变等情况下容易失效，而imu能够高频地获得机器人内部的运动信息，并且不受周围环境的影响，从而弥补相机的不足。同时，相机能够获得丰富的环境信息，通过视觉匹配完成回环检测与回环校正，从而有效地修正imu的累计漂移误差，因此相机与imu的融合被认为具有很大的潜力实现低成本且高精度的定位及图像测量。

以相机标定为例，在图像测量中，为确定空间物体表面某点的三维几何位置与其在图像中对应点之间的相互关系，必须建立相机成像的几何模型，这些几何模型参数就是相机参数。这个求解参数的过程就称之为相机标定，其标定结果的精度直接影响测量结果的准确性。

传统的相机和imu参数标定方法为离线标定法，离线标定法或要求传感器模块静止不动、传感器模块绕imu进行旋转；或需要构造复杂的优化问题来进行相机与imu外参以及imu偏差的标定。

然而在相机和imu的实际使用中，imu的偏差值会受到温度等因素的影响发生缓慢变化。同时，由于机械振动及加工技术等等的限制，相机及imu之间在设备运行过程中并非严格的刚体连接，外参的旋转平移可能产生微小的变化，故，传统的相机和imu参数标定方法为离线标定法精确度较低。

因此，如何实现相机与imu参数的在线标定，是本领域技术人员研究的热门课题。

技术实现要素：

本发明的主要目的在于提供一种标定方法、装置及飞行器，旨在实现相机与imu参数的精准在线标定。

为实现上述目的，本发明提供一种标定方法，应用于飞行器，所述方法包括：

获取所述相机的第一位姿参数，其中，所述第一位姿参数包括所述相机在世界坐标系的位置参数和姿态参数；

获取所述imu的第二位姿参数，其中，所述第二位姿参数包括所述imu在世界坐标系的位置参数和姿态参数；

构建系统状态模型；

根据所述第一位姿参数、所述第二位姿参数及所述状态模型构建系统测量模型；

根据所述系统状态模型和所述系统测量模型构建非线性卡尔曼滤波器；

利用所述非线性卡尔曼滤波器对所述第一位姿参数和所述第二位姿参数进行滤波处理，并输出滤波收敛值，其中，所述滤波收敛值包括所述相机与imu参数的标定估计值。

优选地，所述根据所述第二位姿参数构建系统状态模型，包括：

根据所述第二位姿参数定义系统状态；

根据所述系统状态构建系统状态模型。

优选地，所述系统状态为：

其中，t为向量的转置，^gpi为imu在世界坐标系中的位置；为imu在世界坐标系中的姿态；^gvi为imu在世界坐标系中的速度；ba为imu角速度测量的偏差；bg为imu加速度测量的偏差；ⁱrc为相机与imu外参的平移分量；为相机与imu外参的旋转分量。

优选地，所述根据所述系统状态建立所述系统状态模型，包括：

获取所述^gpi、所述所述^gvi、所述ba、所述bg、所述ⁱrc及所述关于时间的导数，以建立所述系统状态模型。

优选地，所述根据所述第一位姿参数、所述第二位姿参数及所述状态模型构建所述系统测量模型，包括：

选取空间中任意一个空间点在相机坐标系下的坐标构建所述系统测量模型，其中，所述系统测量模型为：

其中，为空间中任意一个空间点的位置；为imu坐标系到相机坐标系的转换矩阵；为世界坐标系到imu坐标系的转换矩阵；为空间点在世界坐标系下的位置；ⁱrc为所述相机与imu外参的平移分量。

本发明还提供一种标定装置，所述装置设置于飞行器，所述飞行器还设置有相机以及imu，所述标定装置包括：

第一位姿模块，用于获取所述相机的第一位姿参数，其中，所述第一位姿参数包括所述相机在世界坐标系的位置参数和姿态参数；

第二位姿模块，用于获取所述imu的第二位姿参数，其中，所述第二位姿参数包括所述imu在世界坐标系的位置参数和姿态参数；

第一建模模块，用于构建系统状态模型；

第二建模模块，用于根据所述第一位姿参数、所述第二位姿参数及所述状态模型构建系统测量模型；

第三建模模块，用于根据所述系统状态模型和所述系统测量模型构建非线性卡尔曼滤波器；

滤波模块，利用所述非线性卡尔曼滤波器对所述第一位姿参数和所述第二位姿参数进行滤波处理，并输出滤波收敛值，其中，所述滤波收敛值包括所述相机与imu参数的标定估计值。

优选地，所述第一建模模块还用于：

根据所述第二位姿参数定义系统状态；

根据所述系统状态构建系统状态模型。

优选地，所述系统状态为：

其中，t为向量的转置，^gpi为imu在世界坐标系中的位置；为imu在世界坐标系中的姿态；^gvi为imu在世界坐标系中的速度；ba为imu角速度测量的偏差；bg为imu加速度测量的偏差；ⁱrc为相机与imu外参的平移分量；为相机与imu外参的旋转分量。

优选地，所述第二建模模块还用于：

获取所述^gpi、所述所述^gvi、所述ba、所述bg、所述ⁱrc及所述关于时间的导数，以建立所述系统状态模型。

本发明还提供一种飞行器，所述飞行器包括机身、与所述机身相连的机臂、设于所述机臂的动力装置、与所述机身相连的相机、与所述相机连接的imu以及与所述相机和imu通信连接的视觉芯片，其特征在于，所述视觉芯片还包括：

存储器以及处理器；

所述存储器用于存储计算机可执行的标定程序；

所述处理器用于调用所述计算机可执行的标定程序以实现前述的标定方法。

与现有技术相比，本发明所提供的一种标定方法通过获取所述相机的第一位姿参数，其中，所述第一位姿参数包括所述相机在世界坐标系的位置参数和姿态参数；获取所述imu的第二位姿参数，其中，所述第二位姿参数包括所述imu在世界坐标系的位置参数和姿态参数；根据所述第二位姿参数构建系统状态模型；根据所述第一位姿参数、所述第二位姿参数及所述状态模型构建系统测量模型；根据所述系统状态模型和所述系统测量模型构建非线性卡尔曼滤波器；利用所述非线性卡尔曼滤波器对所述第一位姿参数和所述第二位姿参数进行滤波处理，并输出滤波收敛值，其中，所述滤波收敛值包括所述相机与imu参数的标定估计值，利用所获取的标定估计值实现对相机与imu参数的精准在线标定。

附图说明

图1为本发明一实施例提供的飞行器与终端设备通信连接的场景示意图；

图2为本发明一实施例提供的标定方法的步骤流程图；

图3为图2中步骤s12的子步骤流程图；

图4为本发明一实施例提供的标定装置的框图结构示意图；

图5为本发明一实施例提供的飞行器的模块框图结构示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本申请的说明书和权利要求书及上述附图中的术语“第一”、“第二”、“第三”、“第四”等(如果存在)是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的实施例能够以除了在这里图示或描述的内容以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或模块的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或模块，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或模块。

需要说明的是，在本发明中涉及“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。另外，各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。

本发明提供了一种标定方法、装置及飞行器，其中，所述标定方法应用于飞行器，所述飞行器设置有相机以及imu，所述标定方法通过获取所述相机的第一位姿参数，其中，所述第一位姿参数包括所述相机在世界坐标系的位置参数和姿态参数；获取所述imu的第二位姿参数，其中，所述第二位姿参数包括所述imu在世界坐标系的位置参数和姿态参数；根据所述第二位姿参数构建系统状态模型；根据所述第一位姿参数、所述第二位姿参数及所述状态模型构建系统测量模型；根据所述系统状态模型和所述系统测量模型构建非线性卡尔曼滤波器；利用所述非线性卡尔曼滤波器对所述第一位姿参数和所述第二位姿参数进行滤波处理，并输出滤波收敛值，其中，所述滤波收敛值包括所述相机与imu参数的标定估计值，利用所获取的标定估计值实现对相机与imu参数的精准在线标定。

请参阅图1，图1为本发明提供的一种飞行器10，该飞行器10与终端设备30通信连接，其中，终端设备30用于控制飞行器10。

该飞行器10可以是旋翼飞行器，如四旋翼飞行器、六旋翼飞行器，也可以是固定以飞行器。终端设备30例如智能手机、平板电脑、电脑、遥控器等。用户可以通过任何合适类型的一种或者多种用户交互设备与终端设备30交互，这些用户交互设备可以是鼠标、按键、触摸屏等。

飞行器10包括机身101、与机身101相连的机臂102、设于机臂102的动力装置103、设于机身101的控制系统(图未示)。动力装置103用于提供飞行器10飞行的推力、升力等，控制系统是飞行器10的中枢神经，可以包括多个功能性模块，例如飞控系统、跟踪系统、路径规划系统以及其他具有特定功能的系统。其中，飞控系统包括各类传感器，例如imu、陀螺仪、加速计等，飞控系统用于控制飞行器10的飞行姿态等。路径规划系统用于基于跟踪目标的位置对飞行器10的飞行路径进行规划，并指示飞控系统控制飞行器10的飞行姿态以使飞行器10按指定路径飞行。其中，跟踪系统包括与机身101连接的相机104和设置于机身101的视觉芯片，相机104和视觉芯片通信连接，相机104用于拍摄获取待跟踪目标的图像或视频等媒体数据，视觉芯片用于确定从媒体数据中识别待跟踪目标，从而生成相应的跟踪控制指令。相机104可以为高清数码相机或其他摄像装置，相机104可以设置于任何利于拍摄的合适位置，在一些实施例中，相机104通过云台安装于机身101的底部。在一些实施例中，视觉芯片也可以设置于机臂102。

视觉芯片可以根据目标的特征对目标利用目标框框选并进行跟踪。在飞行器10的一些应用场景中还包括终端设备30，目标框可以通过终端设备30发送给飞行器10。具体的，终端设备30可以显示飞行器10拍摄的图片，由用户对图片中的待跟踪目标进行框选，获得初始目标框，然后将所述初始目标框上传至飞行器10。

飞行器10和终端设备30之间，可以通过分别设置在各自内部的无线通信模块，例如信号接收器、信号发送器等建立通信连接，上传或者下发数据/指令。在另一些实施例中，初始目标框也可以事先存储于飞行器10的存储装置或视觉芯片中。

请参阅图2，图2为本发明提供的一种标定方法，所述方法应用于飞行器10，由飞行器10的视觉芯片执行，所述方法包括：

步骤s10：获取所述相机的第一位姿参数，其中，所述第一位姿参数包括所述相机在世界坐标系的位置参数和姿态参数。

飞行器10获取设置于飞行器10上的相机104的第一位姿参数，该第一位姿参数为相机104在世界坐标系中的位置参数和姿态参数，其中，位置参数用于表征相机104在世界坐标下的位置坐标；姿态参数用于表征相机104在世界坐标下的姿态角。通过获取相机104在世界坐标系中的位置参数和姿态参数从而可以在世界坐标系中定位到相机104。

步骤s11：获取所述imu的第二位姿参数，其中，所述第二位姿参数包括所述imu在世界坐标系的位置参数和姿态参数。

飞行器10获取设置于飞行器10上的imu的第二位姿参数，该第二位姿参数为imu在世界坐标系中的位置参数和姿态参数，其中，位置参数用于表征imu在世界坐标下的位置坐标；姿态参数用于表征imu在世界坐标下的姿态角。通过获取imu在世界坐标系中的位置参数和姿态参数从而可以在世界坐标系中定位到imu。

步骤s12：根据所述第二位姿参数构建系统状态模型。

请参阅图3，在部分实施例中，所述根据所述第二位姿参数构建系统状态模型，包括：

步骤s121：根据所述第二位姿参数定义系统状态。

根据imu的第二位姿参数定义系统状态，以将imu的第二位姿参数和系统状态关联。

示例性地，定义系统状态为x，且x具有如下表达方程：

其中，t为向量的转置，^gpi为imu在世界坐标系中的位置；为imu在世界坐标系中的姿态；^gvi为imu在世界坐标系中的速度；ba为imu角速度测量的偏差；bg为imu加速度测量的偏差；ⁱrc为相机与imu外参的平移分量；为相机与imu外参的旋转分量，。

步骤s122：根据所述系统状态构建系统状态模型。

系统状态模型是系统状态中所有向量关于时间的导数，获取所述^gpi、所述所述gvi、所述ba、所述bg、所述ⁱrc及所述关于时间的导数，以建立所述系统状态模型。

其中，系统状态模型为：

其中，为关于时间的导数，为^gpi关于时间的导数，为^gvi关于时间的导数，为ba关于时间的导数，为bg关于时间的导数，为ⁱrc关于时间的导数，为关于时间的导数，naω、ngω为预设值，03×1为零矩阵，ⁱω为imu的角速度，ⁱai为imu的加速度，^gg为重力加速度。

步骤s13：根据所述第一位姿参数、所述第二位姿参数及所述状态模型构建系统测量模型。

测量模型是指某种与状态量相关的观测值，本实施例中选取的空间任意一点在相机中的像素坐标作为观测值，构建所述系统测量模型。

示例性地，选取空间中任意一个空间点在相机坐标系下的坐标构建所述系统测量模型，其中，所述系统测量模型为：

其中，为空间中任意一个空间点的位置；为imu坐标系到相机坐标系的转换矩阵；为世界坐标系到imu坐标系的转换矩阵；为空间点在世界坐标系下的位置；ⁱrc为所述相机与imu外参的平移分量。

步骤s14：根据所述系统状态模型和所述系统测量模型构建非线性卡尔曼滤波器。

非线性卡尔曼滤波器可以是扩展卡尔曼滤波器，无迹卡尔曼滤波器或粒子滤波器。

步骤s15：利用所述非线性卡尔曼滤波器对所述第一位姿参数和所述第二位姿参数进行滤波处理，并输出滤波收敛值，其中，所述滤波收敛值包括所述相机与imu参数的标定估计值。

由于飞行器10在运动过程中，设置于飞行器10的相机104和imu跟随相机运动，因此系统状态方程在飞行器10运动过程中实时变化更新。

在部分实施例中，利用所述非线性卡尔曼滤波器对所述第一位姿参数和所述第二位姿参数进行滤波处理，并输出滤波收敛值可以是：

获取非线性卡尔曼滤波器的初始化参数，根据初始化参数初始化非线性卡尔曼滤波器，其中，初始化参数为用户设定的非线性卡尔曼滤波器初始参数；

根据初始化的非线性卡尔曼滤波器对k时刻系统状态模型进行滤波处理，以预测k+1时刻的系统状态模型的预测值和k+1时刻的协方差矩阵的预测值，其中，k时刻系统状态模型根据k时刻的第二位姿参数构建；

根据k+1时刻系统状态模型的预测值和k+1时刻的系统测量模型的测量值输出k+1时刻系统状态模型的估计值，并更新系统状态模型和协方差矩阵，其中，系统测量模型根据所述第一位姿参数、所述第二位姿参数构建；

检测经过n次迭代更新后的系统状态模型和协方差矩阵，若协方差矩阵的预测值小于预设误差值时，判断所述非线性卡尔曼滤波器收敛，并输出滤波收敛值，滤波收敛值包括所述相机与imu参数的标定估计值，利用相机与imu参数的标定估计值可以实现对相机与imu参数精准在线标定。

请参阅图4，本发明还提供一种标定装置20，所述标定装置20设置于飞行器10，所述飞行器10还设置有相机104以及imu，所述标定装置20与所述相机104以及所述imu通信连接，述标定装置20包括：

第一位姿模块201，用于获取所述相机的第一位姿参数，其中，所述第一位姿参数包括所述相机在世界坐标系的位置参数和姿态参数；

第二位姿模块202，用于获取所述imu的第二位姿参数，其中，所述第二位姿参数包括所述imu在世界坐标系的位置参数和姿态参数；

第一建模模块203，用于根据所述第二位姿参数构建系统状态模型；

第二建模模块204，用于根据所述第一位姿参数、所述第二位姿参数及所述状态模型构建系统测量模型；

第三建模模块205，用于根据所述系统状态模型和所述系统测量模型构建非线性卡尔曼滤波器；

滤波模块206，利用所述非线性卡尔曼滤波器对所述第一位姿参数和所述第二位姿参数进行滤波处理，并输出滤波收敛值，其中，所述滤波收敛值包括所述相机与imu参数的标定估计值。

在部分实施例中，第一建模模块还用于：

根据所述第二位姿参数定义系统状态；

根据所述系统状态构建系统状态模型。

在部分实施例中，所述系统状态为：

其中，t为向量的转置，^gpi为imu在世界坐标系中的位置；为imu在世界坐标系中的姿态；^gvi为imu在世界坐标系中的速度；ba为imu角速度测量的偏差；bg为imu加速度测量的偏差；ⁱrc为相机与imu外参的平移分量；为相机与imu外参的旋转分量。

在部分实施例中，第一建模模块还用于：

获取所述^gpi、所述所述^gvi、所述ba、所述bg、所述ⁱrc及所述关于时间的导数，以建立所述系统状态模型。

在部分实施例中，第二建模模块还用于：

选取空间中任意一个空间点在相机坐标系下的坐标构建所述系统测量模型，其中，所述系统测量模型为：

其中，为空间中任意一个空间点的位置；为imu坐标系到相机坐标系的转换矩阵；为世界坐标系到imu坐标系的转换矩阵；为空间点在世界坐标系下的位置；ⁱrc为所述相机与imu外参的平移分量。

请参阅图5，在部分实施例中，飞行器10的视觉芯片还包括存储器105以及处理器106，存储器105与处理器106电连接。

其中，存储器105至少包括一种类型的可读存储介质，所述可读存储介质包括闪存、硬盘、多媒体卡、卡型存储器(例如，sd或dx存储器等)、磁性存储器、磁盘、光盘等。存储器105在一些实施例中可以是飞行器10的内部存储单元，例如该飞行器10的硬盘。存储器105在另一些实施例中也可以是飞行器10的外部存储设备，例如飞行器10上配备的插接式硬盘，智能存储卡(smartmediacard,smc)，安全数字(securedigital,sd)卡，闪存卡(flashcard)等。存储器105不仅可以用于存储安装于飞行器10的应用软件及各类数据，例如计算机可读的标定程序的代码等，还可以用于暂时地存储已经输出或者将要输出的数据。

处理器106在一些实施例中可以是中央处理器(centralprocessingunit,cpu)、控制器、微控制器、微处理器或其他数据处理芯片，处理器106可调用存储器105中存储的程序代码或处理数据，以执行前述的标定方法。

以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。