控制方法、控制装置、可移动平台及存储介质与流程

1.本技术涉及控制技术领域，特别涉及一种控制方法、控制装置、可移动平台和非易失性计算机可读存储介质。


背景技术：

2.随着技术的进步，可移动平台(如无人机、无人车等)，正在替代人类进行更多的生产任务，如可移动平台沿固定轨迹移动，以执行监测、执行预定任务(如洒水等)等，可移动平台移动时，可能因为障碍物等因素，偏移固定轨迹，因此，需要时刻输出可移动平台当前应处于的目标位姿，以对可移动平台的位姿进行调整，然而，gps信号弱、位姿计算失败等因素可能导致位姿的输出帧率降低，使得当前帧输出的目标位姿可能是几帧前可移动平台应处于的目标位姿，位姿调整的准确性较低。


技术实现要素：

3.本技术的实施例提供一种控制方法、控制装置、可移动平台和存储介质。
4.本技术实施例的控制方法包括获取可移动平台采集的当前位姿及当前图像；获取与所述当前图像匹配的地图点；根据所述当前位姿和校正位姿，生成补偿参数，所述校正位姿根据所述地图点确定；及根据所述补偿参数校正所述可移动平台在预定时长内的位姿。
5.本技术实施例的控制装置，应用于可移动平台，所述控制装置包括处理器和存储器，所述存储器用于存储指令，所述处理器调用所述存储器存储的指令用于实现以下操作：获取所述可移动平台采集的当前位姿及当前图像；获取与所述当前图像匹配的地图点；根据所述当前位姿和所述地图点计算得到的校正位姿，生成补偿参数；及根据所述补偿参数校正所述可移动平台在预定时长内的所述当前位姿。
6.本技术实施例的可移动平台包括相机、位姿检测装置和控制装置。所述相机用于采集当前图像，所述位姿检测装置用于采集所述可移动平台的当前姿态，所述控制装置包括处理器，所述处理器用于获取所述当前位姿及所述当前图像；获取与所述当前图像匹配的地图点；根据所述当前位姿和所述地图点计算得到的校正位姿，生成补偿参数；及根据所述补偿参数校正所述可移动平台在预定时长内的所述当前位姿。
7.本技术实施例的非易失性计算机可读存储介质包括计算机程序，当所述计算机程序被一个或多个处理器执行时，使得所述处理器执行控制方法。所述控制方法包括获取可移动平台采集的当前位姿及当前图像；获取与所述当前图像匹配的地图点；根据所述当前位姿和校正位姿，生成补偿参数，所述校正位姿根据所述地图点确定；及根据所述补偿参数校正所述可移动平台在预定时长内的位姿。
8.本技术实施例的控制方法、控制装置、可移动平台和非易失性计算机可读存储介质中，通过获取可移动平台采集的当前位姿和当前图像，来计算与当前图像匹配地图点，从而根据地图点计算校正位姿，从而确定补偿参数，相较于直接计算目标位姿，将可移动平台调整到目标位姿，易受到目标位姿的输出帧率影响导致位姿调整准确地降低而言，通过补
偿参数调整预定时长内的位姿，可防止预定时长内的多帧位姿的偏差不断累积，位姿调整的准确性较高。
附图说明
9.本发明的上述和/或附加的方面和优点从结合下面附图对实施方式的描述中将变得明显和容易理解，其中：
10.图1是本技术实施例提供的可移动平台的结构示意图。
11.图2是本技术实施例提供的控制方法的流程示意图。
12.图3是本技术实施例提供的控制方法的流程示意图。
13.图4是本技术实施例提供的控制方法的流程示意图。
14.图5是本技术实施例提供的控制方法的流程示意图。
15.图6是本技术实施例提供的控制方法的流程示意图。
16.图7是本技术实施例提供的控制方法的流程示意图。
17.图8是本技术实施例提供的控制方法的流程示意图。
18.图9是本技术实施例提供的处理器和计算机可读存储介质的连接示意图。
具体实施方式
19.下面详细描述本技术的实施方式，实施方式的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施方式是示例性的，仅用于解释本技术，而不能理解为对本技术的限制。
20.在本技术的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。在本技术的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是机械连接，也可以是电连接或可以相互通信；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本技术中的具体含义。
21.目前，无人机有很多应用无需用户手动操作，能够通过自动飞行实现。例如，一个或多个无人机沿预定飞行路线飞行，以完成如在农场的农作物所在的区域进行洒水任务、喷农药任务等。再例如，无人机执行一键短片渐远任务；因此，为了保证无人机执行固定轨迹任务时不偏离预定飞行路线，执行一键短片渐远任务时，起点和返回点一致，需要检测无人机在任一位置时的位姿，从而实现位姿调整，然而，gps信号弱、位姿计算失败等因素导致无人机的位姿输出帧率降低，输出的位姿可能并不是当前帧应处于的位姿，而是几帧前的无人机应处于的位姿，位姿调整准确性较差。
22.请参阅图1和图2，本技术实施例提供一种控制方法，应用于控制装置100，该控制方法包括：
23.011：获取可移动平台1000采集的当前位姿及当前图像；
24.012：获取与当前图像匹配的地图点；
25.013：根据当前位姿和校正位姿，生成补偿参数，校正位姿根据地图点确定；及
26.014：根据补偿参数校正可移动平台1000在预定时长内的位姿。
27.本技术实施例还提供一种可移动平台1000，包括相机200、位姿检测装置300和控制装置100。相机200用于采集当前图像，位姿检测装置300用于采集可移动平台1000的当前姿态，控制装置100包括处理器10和存储器20，存储器20用于存储指令，处理器10调用存储器20存储的指令用于获取当前位姿及当前图像；获取与当前图像匹配的地图点；根据当前位姿和地图点计算得到的校正位姿，生成补偿参数；及根据补偿参数校正可移动平台1000在预定时长内的当前位姿。也即是说，步骤011、步骤012、步骤013和步骤014可以由处理器10实现。
28.具体地，可移动平台1000包括无人机、无人驾驶车辆、或地面遥控机器人，以可移动平台1000为无人机，并执行重复轨迹任务(如沿预定轨迹飞行)为例，在无人机移动时，搭载在无人机上的相机200实时拍摄当前图像，相机200可朝向地面进行拍摄，如相机200的光轴垂直地面进行拍摄，从而获取更大范围的地面的图像；位姿检测装置300实时检测无人机的当前位姿，位姿检测装置300可包括gps定位模块和姿态检测模块(如惯性测量单元(inertial measurement unit，imu))，gps定位模块可获取到无人机当前的三维位置坐标，姿态检测模块可获取无人机的姿态，如无人机的俯仰角、横滚角和偏航角。
29.当前位姿和当前图像的采集帧率可相同或不同。当前图像的采集帧率指的是每秒内采集的图像帧数，当前位姿的采集帧率指的是每秒内检测得到的三维位置坐标和姿态的数量。当前位姿和当前图像的采集帧率相同，则当前位姿和当前图像可一一对应。当前位姿和当前图像的采集帧率不相同，如当前位姿的采集帧率大于当前图像的采集帧率时，则可将采集时间与每帧当前图像的采集时间的时间差最小的当前位姿一一对应；或者，如当前位姿的采集帧率小于当前图像的采集帧率时，则可将采集时间与每帧当前位姿的采集时间的时间差最小的当前图像一一对应，从而使得当前图像和当前位姿一一对应，方便后续的计算。
30.处理器10在每次获取到当前图像后，将当前图像与预先建立的重复轨迹所在的区域的地图数据库进行匹配，从而确定地图数据库中与当前图像匹配的地图点，如在地图数据库中确定当前图像匹配的匹配图像，从而获取到匹配图像对应的地图点。
31.地图点可指示无人机在建立地图数据库时，无人机位于地图上的一些特征点时，应处于的位姿。因此，在获取到当前图像匹配的地图点后，可根据当前图像匹配的地图点计算校正位姿。例如，基于pnp算法，通过当前图像匹配的地图点，即可计算得到校正位姿。另外，还可通过目标函数来对校正位姿进行优化，以最小化重投影误差，目标函数如下：
[0032][0033]
其中，t1表示校正位姿，ni为当前图像的特征点集合fc中的一个，n
ij
为地图点mj在第i个当前图像上的观测点，π(t1mj)为针孔相机的投影模型，指将一个三维点投影到图像平面上，形成投影点。如此，可对校正位姿的三维位置坐标和姿态信息进行重投影误差校正，提升位姿校正的准确性。
[0034]
然后根据当前位姿和校正位姿的差异，计算补偿参数。例如，建立当前位姿和校准位姿的映射函数，以作为补偿参数。
[0035]
然后，处理器10基于补偿参数对当前位姿进行校正，使得当前位姿在预定时长内
的位姿均处于校正位姿，预定时长可以是2帧、3帧、4帧、10帧等，每帧的时长可与当前图像或当前位姿的采集帧数对应的每帧时长相同。即使由于gps信号弱、校正位姿计算失败等导致校正位姿的输出帧率降低，也由于位姿调整是根据补偿参数而进行的，可防止预定时长内多帧位姿的偏差不断累积，位姿调整的准确性较高。
[0036]
本技术实施例的控制方法、控制装置100和可移动平台1000中，通过获取可移动平台1000采集的当前位姿和当前图像，来计算与当前图像匹配地图点，从而根据地图点计算校正位姿，从而确定补偿参数，相较于直接计算目标位姿，将可移动平台1000调整到目标位姿，易受到目标位姿的输出帧率影响导致位姿调整准确地降低而言，通过补偿参数调整预定时长内的位姿，可防止预定时长内的多帧位姿的偏差不断累积，位姿调整的准确性较高。
[0037]
请参阅图1和3，在一些实施例中，步骤012还包括：
[0038]
0121：获取与当前图像匹配的匹配图像及与匹配图像对应的地图点；
[0039]
0122：对当前图像和匹配图像进行特征匹配，以确定匹配图像中与当前图像匹配的匹配特征点；
[0040]
0123：获取匹配特征点对应的地图点。
[0041]
在一些实施例中，处理器10调用存储器20存储的指令还用于获取与当前图像匹配的匹配图像及与匹配图像对应的地图点；对当前图像和匹配图像进行特征匹配，以确定匹配图像中与当前图像匹配的匹配特征点；获取匹配特征点对应的地图点。也即是说，步骤0121、步骤0122和步骤0123可以由处理器10实现。
[0042]
具体地，在获取与当前图像匹配的地图点时，可首先在地图数据库中获取与当前图像匹配的匹配图像，匹配图像包括多个特征点，每个特征点均对应一个地图点，当前图像和匹配图像匹配可以是当前图像和匹配图像的相似度大于预定相似度(如80％、90％等)，因此，匹配图像中可能存在与当前图像不匹配的特征点，在对当前图像和匹配图像进行特征点匹配后，即可确定匹配图像中与当前图像匹配的匹配特征点，从而确定匹配特征点对应的地图点。如此，可确定与当前图像匹配的地图点(即匹配特征点对应的地图点)，地图点可包括特征点及特征点对应的位姿信息。
[0043]
请参阅图1和图4，在某些实施例中，当前位姿包括第一位置信息和第一姿态信息，第一姿态信息包括第一俯仰角、第一横滚角和第一偏航角，校正位姿包括第二位置信息和第二姿态信息，第二姿态信息包括第二俯仰角、第二横滚角和第二偏航角，步骤013包括：
[0044]
0131：将第二俯仰角和第二横滚角分别替换为第一俯仰角和第一横滚角，以生成替换后的第二姿态信息；及
[0045]
0132：根据第一位置信息、第一姿态信息、第二位置信息、和替换后的第二姿态信息，计算补偿参数。
[0046]
在某些实施例中，处理器10调用存储器20存储的指令还用于将第二俯仰角和第二横滚角分别替换为第一俯仰角和第一横滚角，以生成替换后的第二姿态信息；及根据第一位置信息、第一姿态信息、第二位置信息、和替换后的第二姿态信息，计算补偿参数。也即是说，步骤0131和步骤0132可以由处理器10实现。
[0047]
具体地，在计算补偿参数时，考虑到imu测量的俯仰角和横滚角基本不存在误差，精度较高，偏差的累积主要在三维位置坐标和偏航角，故可将校正位姿中的第二俯仰角和第二横滚角分别替换为imu当前采集的第一俯仰角和第一横滚角，以生成替换后的第二姿
态信息；从而使得无人机校正前后的俯仰角和横滚角基本不变。
[0048]
然后根据当前位姿的第一位置信息、第一姿态信息、校正位姿的第二位置信息、及替换后的第二姿态信息，来计算补偿参数。例如，处理器10可根据如下公式计算得到补偿参数：
[0049]
△
t＝t2*t1-1
；其中，
△
t表示补偿参数，t1表示当前位姿、t2表示替换后的校正位姿。如此，可快速计算得到补偿参数。补偿参数可包括多个，每个补偿参数对应一个位姿参数，三维位置坐标包括x、y、z三个位置参数，当前姿态包括横滚角(roll)、俯仰角(pitch)和偏航角(yaw)三个姿态参数，也即是说，可通过位置参数对当前位姿中的位置坐标进行校正，通过姿态参数对当前位姿中的姿态进行校正。如此，防止引入pitch和roll的误差，提升位姿校正的准确性。
[0050]
请参阅图1和图5，在某些实施例中，控制方法还包括：
[0051]
015：根据可移动平台1000采集的多帧采集图像及采集图像对应的位姿信息，建立地图数据库。
[0052]
在某些实施例中，处理器10调用存储器20存储的指令还用于根据可移动平台1000采集的多帧采集图像及采集图像对应的位姿信息，建立地图数据库。也即是说，步骤015可以由处理器10实现。
[0053]
具体地，在进行重复轨迹任务时，需要实现建立重复轨迹的全局地图，以生成地图数据库。无人机在重复轨迹任务对应的路径上移动，并控制相机200不断采集采集图像及位姿信息，采集图像和位姿信息可一一对应，方便后续对地图点的计算。
[0054]
在无人机从轨迹的起点移动到终点后，处理器10可将移动期间采集的多帧采集图像进行融合，从而生成全局地图，且全局地图的不同区域均存在对应的位姿信息，从而实现全局地图的建立，根据全局地图及全局地图对应的位姿信息即可建立地图数据库。
[0055]
请参阅图1和图6，在某些实施例中，步骤015包括：
[0056]
0151：获取采集采集图像时，可移动平台1000的位姿信息，以绑定采集图像和位姿信息；
[0057]
0152：识别采集图像的特征点；
[0058]
0153：根据特征点和特征点对应的位姿信息计算地图点；及
[0059]
0154：根据采集图像和地图点建立地图数据库。
[0060]
在某些实施例中，处理器10调用存储器20存储的指令还用于获取采集采集图像时，可移动平台1000的位姿信息，以绑定采集图像和位姿信息；识别采集图像的特征点；根据特征点和特征点对应的位姿信息计算地图点；及根据采集图像和地图点建立地图数据库。也即是说，步骤0151、步骤0152、步骤0153和步骤0154可以由处理器10实现。
[0061]
具体地，在建立地图数据库时，可将获取采集图像时，位姿检测装置300检测得到的位姿信息绑定，以实现采集图像和姿态信息的一一对应。
[0062]
然后处理器10识别采集图像中的特征点，根据特征点和特征点对应的位姿信息计算地图点，例如，处理器10可将连续多帧采集图像对齐，然后根据连续多帧采集图像中表示同一对象的特征点对应的位姿信息来计算特征点对应的地图点，最后将多帧采集图像融合先生成全局地图，并确定全局地图中的每个特征点对应的地图点，将全局地图、特征点、及特征点对应的地图点关联存储，以生成地图数据库。
[0063]
请参阅图1和图7，在某些实施例中，步骤0153包括：
[0064]
01531：确定多帧采集图像中的关键帧图像和非关键帧图像，关键帧图像为多帧采集图像中的任一帧；
[0065]
01532：将关键帧图像与非关键帧图像进行特征匹配，以获取关键帧图像中匹配成功次数大于预定次数的第一特征点；
[0066]
01533：根据第一特征点和非关键帧图像中与第一特征点匹配成功的第二特征点，计算第三位置信息；及
[0067]
01534：根据第三位置信息和第一特征点对应的位姿信息，生成地图点。
[0068]
在某些实施例中，处理器10调用存储器20存储的指令还用于确定多帧采集图像中的关键帧图像和非关键帧图像，关键帧图像为多帧采集图像中的任一帧；将关键帧图像与非关键帧图像进行特征匹配，以获取关键帧图像中匹配成功次数大于预定次数的第一特征点；根据第一特征点和非关键帧图像中与第一特征点匹配成功的第二特征点，计算第三位置信息；及根据第三位置信息和第一特征点对应的位姿信息，生成地图点。也即是说，步骤01531、步骤01532、步骤01533和步骤01534可以由处理器10实现。
[0069]
具体地，在根据特征点对应的位姿信息计算地图点时，由于连续多帧采集图像的相似度较高，存在相同特征点的几率较高，因此，可在连续多帧采集图像中确定关键帧图像和非关键帧图像，关键帧可以是连续多帧采集图像中的任一帧。
[0070]
然后将关键帧和非关键帧进行特征匹配，从而在非关键帧中找到与关键帧的特征点匹配的特征点，若关键帧的特征点在预定个数的非关键帧中均存在匹配的特征点，则确定该关键帧的特征点为匹配成功次数大于预定次数的第一特征点，其中，预定个数的数值等于预定次数的数值，预定个数可以是3个、4个、5个等，预定个数越多，计算得到的地图点的准确性越高。
[0071]
在确定关键帧图像中的多个第一特征点和非关键帧中与第一特征点对应的第二特征点后，即可根据第一特征点和对应的多个第二特征点，计算第一特征点的第三位置信息，例如处理器10可根据pnp算法计算第三位置信息，然后处理器10根据第一特征点对应的第三位置信息和位姿信息，即可生成地图点，可以理解，位姿信息中的三维位置坐标为对应的采集图像的中心的三维位置坐标，而第一特征点可能并不在采集图像的中心，因此，可使用第三位置信息替换位姿信息中的三维位置坐标，从而生成第一特征点对应的地图点。如此，可获取关键帧图像中所有第一特征点对应的地图点。
[0072]
请参阅图1和图8，在某些实施例中，控制方法还包括：
[0073]
016：根据关键帧图像，建立词袋数据库；
[0074]
步骤0121包括：
[0075]
01211：获取词袋数据库中，与当前图像匹配的关键帧图像，以作为匹配图像；及
[0076]
01212：获取匹配图像包含的第一特征点对应的地图点。
[0077]
在某些实施例中，处理器10调用存储器20存储的指令还用于根据关键帧图像，建立词袋数据库；获取词袋数据库中，与当前图像匹配的关键帧图像，以作为匹配图像；及获取匹配图像包含的第一特征点对应的地图点。也即是说，步骤016、步骤01211和步骤01212可以由处理器10实现。
[0078]
具体地，在获取到关键帧图像及关键帧图像的第一特征点后，可根据关键帧图像
建立词袋数据库，以用于与当前图像进行特征匹配。在匹配时，可识别当前图像的特征点，然后在词袋数据库中找到与当前图像的特征点的相似度最高的关键帧图像，以作为匹配图像，然后从地图数据库中获取该匹配图像中的第一特征点对应的地图点，即可获取与当前图像匹配的地图点。如此，通过存储关键帧图像的词袋数据库可快速实现特征匹配，找到与当前图像匹配的关键帧图像，快速获取与当前图像匹配的地图点。
[0079]
在一些实施例中，可对预定时间内获取的关键帧图像和对应的地图点进行优化，具体通过优化函数来实现，如将关键帧图像及对应的地图点输入优化函数，输出以使得优化函数输出值最小的关键帧图像及对应的地图点，从而实现关键帧图像和对应的地图点的优化。优化函数如下：
[0080][0081]eij
＝||n
ij-π(rimj ti)||2[0082]
其中，kfi表示预定时间内的关键帧集合kf
set
中的一帧，mj为预定时间内的地图点集合m
set
中的一个，n
ij
为地图点mj在第i个当前图像上的观测点，π(rimj tj)为针孔相机的投影模型，指将一个三维点投影到图像平面上，形成投影点，ri，ti表示将地图点mj从世界坐标系变换到图像坐标系的映射关系，eij为投影点和观测点之间的二范数误差，为优化函数的输出值。
[0083]
在一些实施例中，无人机建立重复轨迹任务的地图数据库时，轨迹的起点和终点可能是同一位置，而无人机到达终点时，可能与起点的位置存在偏差，因此，当获取到当前关键帧图像时，会将当前关键帧图像与词袋数据库中的已存储的关键帧图像进行特征匹配，从而确定当前关键帧图像和已存储的关键帧图像的相似度，若存在相似度大于相似度阈值(如90％、95％等)的已存储的关键帧图像，则可确定无人机已回到终点，但位置存在一定偏差，因此，处理器10可根据相似度大于相似度阈值的已存储的关键帧图像来对当前关键帧图像进行校正，如根据相似度大于预设相似度阈值的关键帧图像和当前关键帧图像的映射关系，对关键帧图像进行校正，并根据词袋数据库中相似度大于预设相似度阈值的关键帧图像对应的地图点和当前关键帧图像对应的地图点的映射关系，对当前关键帧图像对应的地图点进行校正。例如，将当前关键帧图像使用相似度大于相似度阈值的已存储的关键帧图像替换，当前关键帧图像对应的地图点使用相似度大于相似度阈值的已存储的关键帧图像对应的地图点替换。
[0084]
请参阅图9，本技术实施例还提供一种包含计算机程序402的非易失性计算机可读存储介质400，当计算机程序402被一个或多个处理器10执行时，使得处理器10执行上述任一实施例的控制方法。
[0085]
例如，请结合图2，当计算机程序402被一个或多个处理器10执行时，使得处理器10执行以下步骤：
[0086]
011：获取可移动平台采集的当前位姿及当前图像；
[0087]
012：获取与当前图像匹配的地图点；
[0088]
013：根据当前位姿和校正位姿，生成补偿参数，校正位姿根据地图点确定；及
[0089]
014：根据补偿参数校正可移动平台在预定时长内的位姿。
[0090]
再例如，请结合图3，当计算机程序402被一个或多个处理器10执行时，使得处理器10执行以下步骤：
[0091]
0121：获取与当前图像匹配的匹配图像及与匹配图像对应的地图点；
[0092]
0122：对当前图像和匹配图像进行特征匹配，以确定匹配图像中与当前图像匹配的匹配特征点；
[0093]
0123：获取匹配特征点对应的地图点。
[0094]
可以理解，各个实施例对应的示意图中包含有执行动作的时序时，该时序仅为示例性说明，根据需要，各个执行动作之前的时序可以有变化，同时，各个实施例之间，在不矛盾冲突的情况下，可以结合或拆分为一个或多个实施例，以适应不同的应用场景，此处不做赘述。
[0095]
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示意性实施例”、“一个例子”、“具体示例”、或“一些示例”等的描述意指结合实施例或示例描述的具体特征、结构、材料或者特点包含于本技术的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
[0096]
流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于执行特定逻辑功能或过程的步骤的程序的代码的模块、片段或部分，并且本技术的优选实施例的范围包括另外的执行，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本技术的实施例所属技术领域的技术人员所理解。
[0097]
在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于执行逻辑功能的程序的定序列表，可以具体执行在任何计算机可读介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器10210的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。
[0098]
尽管上面已经示出和描述了本技术的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本技术的限制，本领域的普通技术人员在本技术的范围内可以对上述实施例进行变化、修改、替换和变型。