无人驾驶汽车的定位方法及装置与流程

本发明涉及无人驾驶技术领域，特别是涉及一种无人驾驶汽车的定位方法及装置。

背景技术：

随着无人驾驶技术的不断发展和日益普及，越来越多的封闭或半封闭场景开始采用无人驾驶技术对作业过程进行辅助和提供支持，港口环境便是其中之一。在无人驾驶汽车进行作业的过程中，无人驾驶汽车需要实时获取自身的定位结果，从而保证自身安全和工作稳定性，因此，如何精准的对无人驾驶汽车的位姿进行定位，从而获取无人驾驶汽车的定位结果是至关重要的。

目前，无人驾驶汽车通常会通过差分gps定位法、激光定位法及视觉定位法三种定位方法中的任意一种定位方法获取无人驾驶汽车的定位结果，然而，由于港口环境较为复杂，以及港口环境内的轨道吊和集装箱位置变化频繁，使得现有的定位方法无法精准的对无人驾驶汽车的位姿进行定位，进而导致无人驾驶汽车通过现有的定位方法获取得到的定位结果的准确性较低。

技术实现要素：

有鉴于此，本发明提供一种无人驾驶汽车的定位方法及装置，主要目的在于精准的对无人驾驶汽车的位姿进行定位，从而获取得到准确性较高的定位结果。

为了达到上述目的，本发明主要提供如下技术方案：

第一方面，本发明提供了一种无人驾驶汽车的定位方法，该方法包括：

获取当前控制量、历史观测位姿状态、历史预测位姿状态及历史预测协方差矩阵，并根据所述当前控制量、所述历史观测位姿状态、所述历史预测位姿状态及所述历史预测协方差矩阵，计算时间更新位姿状态及时间更新协方差矩阵；

获取当前观测定位结果；

获取当前观测矩阵及当前观测协方差矩阵，并将所述当前观测矩阵、所述当前观测协方差矩阵及所述时间更新协方差矩阵代入第一预设算法中，计算所述当前卡尔曼增益矩阵；

将所述时间更新位姿状态、所述当前卡尔曼增益矩阵及所述当前观测定位结果代入第二预设算法中，计算当前预测位姿状态。

可选的，所述根据所述当前控制量、所述历史观测位姿状态、所述历史预测位姿状态及所述历史预测协方差矩阵，计算时间更新位姿状态及时间更新协方差矩阵，包括：

使用预设函数对所述历史观测位姿状态进行偏导处理，以获得所述历史观测位姿状态对应的第一雅可比矩阵；

使用所述预设函数对所述当前控制量进行偏导处理，以获得所述当前控制量对应的第二雅可比矩阵；

将所述第一雅可比矩阵、所述第二雅可比矩阵、所述当前控制量及所述历史预测位姿状态代入第三预设算法中，计算所述时间更新位姿状态；

将所述第一雅可比矩阵、所述第二雅可比矩阵、所述历史预测协方差矩阵及自定义代数量代入第四预设算法中，计算所述时间更新协方差矩阵。

可选的，所述获取当前观测定位结果，包括：

将所述时间更新位姿状态代入第五预设算法中，计算当前预测定位结果；

获取第一当前定位结果，并计算所述第一当前定位结果与所述当前预测定位结果之间的第一差值；

若所述第一差值小于或等于预设阈值向量，则将所述第一当前定位结果确定为所述当前观测定位结果；

若所述第一差值大于所述预设阈值向量，则获取第二当前定位结果，并计算所述第二当前定位结果与所述当前预测定位结果之间的第二差值；

若所述第二差值小于或等于所述预设阈值向量，则将所述第二当前定位结果确定为所述当前观测定位结果；

若所述第二差值大于所述预设阈值向量，则获取第三当前定位结果，并计算所述第三当前定位结果与所述当前预测定位结果之间的第三差值；

若所述第三差值小于或等于所述预设阈值向量，则将所述第三当前定位结果确定为所述当前观测定位结果；

若所述第三差值大于所述预设阈值向量，则将所述第一差值、第二差值及第三差值中的最小值对应的当前定位结果确定为所述当前观测定位结果；其中，所述第一当前定位结果为基于差分gps定位法、激光定位法及视觉定位法获取得到的第一个定位结果，所述第二当前定位结果为基于差分gps定位法、激光定位法及视觉定位法获取得到的第二个定位结果，所述第三当前定位结果为基于差分gps定位法、激光定位法及视觉定位法获取得到的第三个定位结果。

可选的，在所述将所述时间更新位姿状态、所述当前卡尔曼增益矩阵及所述当前观测定位结果代入第二预设算法中，计算当前预测位姿状态之后，所述方法还包括：

将所述当前观测矩阵、所述当前卡尔曼增益矩阵及所述时间更新协方差矩阵代入第六预设算法中，计算当前预测协方差矩阵；

将所述当前预测位姿状态、所述当前预测协方差矩阵及所述当前观测定位结果缓存至本地存储空间中。

可选的，在所述将所述时间更新位姿状态、所述当前卡尔曼增益矩阵及所述当前观测定位结果代入第二预设算法中，计算当前预测位姿状态之后，所述方法还包括：

将所述当前预测位姿状态进行输出显示。

第二方面，本发明还提供一种无人驾驶汽车的定位装置，该装置包括：

第一获取单元，用于获取当前控制量、历史观测位姿状态、历史预测位姿状态及历史预测协方差矩阵；

第一计算单元，用于根据所述当前控制量、所述历史观测位姿状态、所述历史预测位姿状态及所述历史预测协方差矩阵，计算时间更新位姿状态及时间更新协方差矩阵；

第二获取单元，用于获取当前观测定位结果；

第三获取单元，用于获取当前观测矩阵及当前观测协方差矩阵；

第二计算单元，用于将所述当前观测矩阵、所述当前观测协方差矩阵及所述时间更新协方差矩阵代入第一预设算法中，计算所述当前卡尔曼增益矩阵；

第三计算单元，用于将所述时间更新位姿状态、所述当前卡尔曼增益矩阵及所述当前观测定位结果代入第二预设算法中，计算当前预测位姿状态。

可选的，所述第一计算单元包括：

第一偏导模块，用于使用预设函数对所述历史观测位姿状态进行偏导处理，以获得所述历史观测位姿状态对应的第一雅可比矩阵；

第二偏导模块，用于使用所述预设函数对所述当前控制量进行偏导处理，以获得所述当前控制量对应的第二雅可比矩阵；

第一计算模块，用于将所述第一雅可比矩阵、所述第二雅可比矩阵、所述当前控制量及所述历史预测位姿状态代入第三预设算法中，计算所述时间更新位姿状态；

第二计算模块，用于将所述第一雅可比矩阵、所述第二雅可比矩阵、所述历史预测协方差矩阵及自定义代数量代入第四预设算法中，计算所述时间更新协方差矩阵。

可选的，所述第二获取单元包括：

第三计算模块，用于将所述时间更新位姿状态代入第五预设算法中，计算当前预测定位结果；

第四计算模块，用于获取第一当前定位结果，并计算所述第一当前定位结果与所述当前预测定位结果之间的第一差值；

第一确定模块，用于当所述第一差值小于或等于预设阈值向量时，将所述第一当前定位结果确定为所述当前观测定位结果；

第五计算模块，用于当所述第一差值大于所述预设阈值向量时，获取第二当前定位结果，并计算所述第二当前定位结果与所述当前预测定位结果之间的第二差值；

第二确定模块，用于当所述第二差值小于或等于所述预设阈值向量时，将所述第二当前定位结果确定为所述当前观测定位结果；

第六计算模块，用于当所述第二差值大于所述预设阈值向量时，获取第三当前定位结果，并计算所述第三当前定位结果与所述当前预测定位结果之间的第三差值；

第三确定模块，用于当所述第三差值小于或等于所述预设阈值向量时，将所述第三当前定位结果确定为所述当前观测定位结果；

第四确定模块，用于当所述第三差值大于所述预设阈值向量时，将所述第一差值、第二差值及第三差值中的最小值对应的当前定位结果确定为所述当前观测定位结果；其中，所述第一当前定位结果为基于差分gps定位法、激光定位法及视觉定位法获取得到的第一个定位结果，所述第二当前定位结果为基于差分gps定位法、激光定位法及视觉定位法获取得到的第二个定位结果，所述第三当前定位结果为基于差分gps定位法、激光定位法及视觉定位法获取得到的第三个定位结果。

可选的，所述装置还包括：

第四计算单元，用于在所述第三计算单元将所述时间更新位姿状态、所述当前卡尔曼增益矩阵及所述当前观测定位结果代入第二预设算法中，计算当前预测位姿状态之后，将所述当前观测矩阵、所述当前卡尔曼增益矩阵及所述时间更新协方差矩阵代入第六预设算法中，计算当前预测协方差矩阵；

缓存单元，用于将所述当前预测位姿状态、所述当前预测协方差矩阵及所述当前观测定位结果缓存至本地存储空间中。

可选的，所述装置还包括：

输出单元，用于在所述第三计算单元将所述时间更新位姿状态、所述当前卡尔曼增益矩阵及所述当前观测定位结果代入第二预设算法中，计算当前预测位姿状态之后，将所述当前预测位姿状态进行输出显示。

为了实现上述目的，根据本发明的第三方面，提供了一种存储介质，所述存储介质包括存储的程序，其中，在所述程序运行时控制所述存储介质所在设备执行上述所述的无人驾驶汽车的定位方法。

为了实现上述目的，根据本发明的第四方面，提供了一种处理器，所述处理器用于运行程序，其中，所述程序运行时执行上述所述的无人驾驶汽车的定位方法。

借由上述技术方案，本发明提供的技术方案至少具有下列优点：

本发明提供的一种无人驾驶汽车的定位方法及装置，与现有技术中无人驾驶汽车基于差分gps定位法、激光定位法及视觉定位法三种定位方法中的任意一种定位方法获取自身对应的定位结果相比，本发明能够使无人驾驶汽车先获取自身对应的当前控制量、历史观测位姿状态、历史预测位姿状态及历史预测协方差矩阵，再根据获取的当前控制量、历史观测位姿状态、历史预测位姿状态及历史预测协方差矩阵，计算自身对应的时间更新位姿状态及时间更新协方差矩阵，并在获取得到自身对应的当前观测定位结果后，获取自身对应的当前观测矩阵及当前观测协方差矩阵，并根据获取的当前观测矩阵、当前观测协方差矩阵及时间更新协方差矩阵，计算自身对应的当前卡尔曼增益矩阵，以及根据计算获得的当前卡尔曼增益矩阵、时间更新位姿状态及当前观测定位结果，计算自身对应的当前预测位姿状态。由于，无人驾驶汽车是基于自身对应的当前观测定位结果、当前卡尔曼增益矩阵以及时间更新位姿状态，计算自身对应的当前预测位姿状态，即基于ekf(extendedkalmanfilter,扩展卡尔曼滤波器)框架预测当前时刻自身对应的定位结果，因此，无人驾驶汽车在复杂的港口环境内也能够精准的对自身的位姿进行定位，从而能够保证无人驾驶汽车在作业过程中的安全性和工作稳定性。

上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，而可依照说明书的内容予以实施，并且为了让本发明的上述和其它目的、特征和优点能够更明显易懂，以下特举本发明的具体实施方式。

附图说明

通过阅读下文优选实施方式的详细描述，各种其他的优点和益处对于本领域普通技术人员将变得清楚明了。附图仅用于示出优选实施方式的目的，而并不认为是对本发明的限制。而且在整个附图中，用相同的参考符号表示相同的部件。在附图中：

图1示出了本发明实施例提供的一种无人驾驶汽车的定位方法流程图；

图2示出了本发明实施例提供的另一种无人驾驶汽车的定位方法流程图；

图3示出了本发明实施例提供的一种无人驾驶汽车的定位装置的组成框图；

图4示出了本发明实施例提供的另一种无人驾驶汽车的定位装置的组成框图。

具体实施方式

下面将参照附图更详细地描述本公开的示例性实施例。虽然附图中显示了本公开的示例性实施例，然而应当理解，可以以各种形式实现本公开而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本公开，并且能够将本公开的范围完整的传达给本领域的技术人员。

本发明实施例提供一种无人驾驶汽车的定位方法，如图1所示，该方法包括：

101、获取当前控制量、历史观测位姿状态、历史预测位姿状态及历史预测协方差矩阵，并根据当前控制量、历史观测位姿状态、历史预测位姿状态及历史预测协方差矩阵，计算时间更新位姿状态及时间更新协方差矩阵。

其中，无人驾驶汽车对应的当前控制量为上一时刻到当前时刻无人驾驶汽车角速度和加速度的变化值，具体的，无人驾驶汽车可以通过惯性测量单元(inertialmeasurementunit，imu)获取自身对应的当前控制量；其中，无人驾驶汽车对应的观测定位结果为基于现有的定位方法获取得到的定位结果，其包含无人驾驶汽车对应的朝向及位置，无人驾驶汽车对应的观测位姿状态为对观测定位结果进行转换处理得到的定位结果，其包含无人驾驶汽车对应的朝向、位置及速度，从而无人驾驶汽车对应的历史观测位姿状态为上一时刻无人驾驶汽车对应的观测位姿状态，其包含上一时刻无人驾驶汽车对应的朝向、位置及速度；其中，无人驾驶汽车对应的预测位姿状态为进行预测无人驾驶汽车位姿状态操作的过程中，计算得到的定位结果，其包含无人驾驶汽车对应的朝向、位置及速度，从而无人驾驶汽车对应的历史预测位姿状态为上一时刻无人驾驶汽车对应的预测位姿状态；其中，无人驾驶汽车对应的预测协方差矩阵为进行预测无人驾驶汽车位姿状态操作的过程中，计算得到的协方差矩阵，从而无人驾驶汽车对应的历史预测协方差矩阵为上一时刻无人驾驶汽车对应的预测协方差矩阵。

在本发明实施例中，无人驾驶汽车在基于现有的定位方法获取得到一个观测定位结果，并基于该观测定位结果对自身的位姿状态进行初始化后，便可基于自身当前时刻对应的观测定位结果(即当前观测定位结果)，计算当前时刻自身对应的预测位姿状态(即当前预测位姿状态)。而无人驾驶汽车在基于自身对应的当前观测定位结果，计算自身对应的当前预测位姿状态时，首先需要获取自身对应的当前控制量、历史观测位姿状态、历史预测位姿状态及历史预测协方差矩阵，并对获取得到的历史预测位姿状态进行时间更新处理，从而获得自身对应的时间更新位姿状态，以及对获取得到的历史预测协方差矩阵进行时间更新处理，从而获得自身对应的时间更新协方差矩阵，即根据获取得到的当前控制量、历史观测位姿状态、历史预测位姿状态及历史预测协方差矩阵，计算自身对应的时间更新位姿状态及时间更新协方差矩阵，以便后续基于自身对应的当前观测定位结果、时间更新位姿状态及时间更新协方差矩阵，计算自身对应的当前预测位姿状态。

102、获取当前观测定位结果。

在本发明实施例中，无人驾驶汽车在根据自身对应的当前控制量、历史观测位姿状态、历史预测位姿状态及历史预测协方差矩阵，计算得到自身对应的时间更新位姿状态及时间更新协方差矩阵后，便可获取自身对应的当前观测定位结果，以便后续基于自身对应的当前观测定位结果，计算自身对应的当前预测位姿状态。具体的，在本发明实施例中，无人驾驶汽车可以在基于任意一种现有的定位方法获取得到当前时刻自身对应的定位结果后，将该定位结果确定为自身对应的当前预测位姿状态，也可以在基于每种现有的定位方法获取得到当前时刻自身对应的定位结果后，将最优的定位结果确定为自身对应的当前预测位姿状态，但不限于此。

103、获取当前观测矩阵及当前观测协方差矩阵，并将当前观测矩阵、当前观测协方差矩阵及时间更新协方差矩阵代入第一预设算法中，计算当前卡尔曼增益矩阵。

在本发明实施例中，无人驾驶汽车在获取得到自身对应的当前观测定位结果后，便可获取当前时刻自身对应的观测矩阵及观测协方差矩阵(即当前观测矩阵及当前观测协方差矩阵)，并将获取得到的当前观测矩阵、当前观测协方差矩阵及在步骤101中计算得到的时间更新协方差矩阵代入第一预设算法中，从而计算当前时刻自身对应的卡尔曼增益矩阵(即当前卡尔曼增益矩阵)，其中，第一预设算法具体如下：

kk＝pk|k-1hk^t(hkpk|k-1hk^t+rk)^-1

其中，kk为当前卡尔曼增益矩阵，pk|k-1为时间更新协方差矩阵，hk为当前观测矩阵，rk为当前观测协方差矩阵。

104、将时间更新位姿状态、当前卡尔曼增益矩阵及当前观测定位结果代入第二预设算法中，计算当前预测位姿状态。

在本发明实施例中，无人驾驶汽车在经过步骤103计算得到自身对应的当前卡尔曼增益矩阵后，便可将在步骤101中计算得到的时间更新位姿状态、在步骤103中计算得到的当前卡尔曼增益矩阵以及在步骤102中获取得到的当前观测定位结果代入第二预设算法中，从而计算当前时刻自身对应的预测位姿状态(即当前预测位姿状态)，基于ekf(extendedkalmanfilter,扩展卡尔曼滤波器)框架预测当前时刻自身对应的定位结果，其中，第二预设算法具体如下：

xk|k＝xk|k-1+kk[zk-h(xk|k-1)]

其中，xk|k为当前预测位姿状态，xk|k-1为时间更新位姿状态，kk为当前卡尔曼增益矩阵，zk为当前观测定位结果，h为单位矩阵。

本发明实施例提供的一种无人驾驶汽车的定位方法，与现有技术中无人驾驶汽车基于差分gps定位法、激光定位法及视觉定位法三种定位方法中的任意一种定位方法获取自身对应的定位结果相比，本发明实施例能够使无人驾驶汽车先获取自身对应的当前控制量、历史观测位姿状态、历史预测位姿状态及历史预测协方差矩阵，再根据获取的当前控制量、历史观测位姿状态、历史预测位姿状态及历史预测协方差矩阵，计算自身对应的时间更新位姿状态及时间更新协方差矩阵，并在获取得到自身对应的当前观测定位结果后，获取自身对应的当前观测矩阵及当前观测协方差矩阵，并根据获取的当前观测矩阵、当前观测协方差矩阵及时间更新协方差矩阵，计算自身对应的当前卡尔曼增益矩阵，以及根据计算获得的当前卡尔曼增益矩阵、时间更新位姿状态及当前观测定位结果，计算自身对应的当前预测位姿状态。由于，无人驾驶汽车是基于自身对应的当前观测定位结果、当前卡尔曼增益矩阵以及时间更新位姿状态，计算自身对应的当前预测位姿状态，即基于ekf(extendedkalmanfilter,扩展卡尔曼滤波器)框架预测当前时刻自身对应的定位结果，因此，无人驾驶汽车在复杂的港口环境内也能够精准的对自身的位姿进行定位，从而能够保证无人驾驶汽车在作业过程中的安全性和工作稳定性。

以下为了更加详细地说明，本发明实施例提供了另一种无人驾驶汽车的定位方法，特别是无人驾驶汽车根据当前控制量、历史观测位姿状态、历史预测位姿状态及历史预测协方差矩阵，计算时间更新位姿状态及时间更新协方差矩阵的具体方法以及无人驾驶汽车获取当前观测定位结果的具体方法，具体如图2所示，该方法包括：

201、获取当前控制量、历史观测位姿状态、历史预测位姿状态及历史预测协方差矩阵。

其中，关于步骤201、获取当前控制量、历史观测位姿状态、历史预测位姿状态及历史预测协方差矩阵，可以参考图1对应部分的描述，本发明实施例此处将不再赘述。

202、根据当前控制量、历史观测位姿状态、历史预测位姿状态及历史预测协方差矩阵，计算时间更新位姿状态及时间更新协方差矩阵。

在本发明实施例中，无人驾驶汽车在获取自身对应的当前控制量、历史观测位姿状态、历史预测位姿状态及历史预测协方差矩阵后，便可对获取得到的历史预测位姿状态进行时间更新处理，从而获得自身对应的时间更新位姿状态，以及对获取得到的历史预测协方差矩阵进行时间更新处理，从而获得自身对应的时间更新协方差矩阵(即根据获取得到的当前控制量、历史观测位姿状态、历史预测位姿状态及历史预测协方差矩阵，计算自身对应的时间更新位姿状态及时间更新协方差矩阵)。以下将对无人驾驶汽车如何根据当前控制量、历史观测位姿状态、历史预测位姿状态及历史预测协方差矩阵，计算时间更新位姿状态及时间更新协方差矩阵进行详细说明。

(1)使用预设函数对历史观测位姿状态进行偏导处理，以获得历史观测位姿状态对应的第一雅可比矩阵，以及使用预设函数对当前控制量进行偏导处理，以获得当前控制量对应的第二雅可比矩阵。

在本发明实施例中，无人驾驶汽车在获取自身对应的当前控制量、历史观测位姿状态、历史预测位姿状态及历史预测协方差矩阵后，便可使用预设函数分别获取得到的历史观测位姿状态和当前控制量进行偏导处理，从而获得历史观测位姿状态对应的第一雅可比矩阵和当前控制量对应的第二雅可比矩阵。

(2)将第一雅可比矩阵、第二雅可比矩阵、当前控制量及历史预测位姿状态代入第三预设算法中，计算时间更新位姿状态。

在本发明实施例中，无人驾驶汽车在获取得到历史观测位姿状态对应的第一雅可比矩阵和当前控制量对应的第二雅可比矩阵后，便可基于第一雅可比矩阵和第二雅可比矩阵对历史预测位姿状态进行时间更新处理，从而获得自身对应的时间更新位姿状态，即将第一雅可比矩阵、第二雅可比矩阵、当前控制量及历史预测位姿状态代入第三预设算法中，从而计算时间更新位姿状态，其中，第三预设算法具体如下：

xk|k-1＝axk-1|k-1+buk

其中，xk|k-1为时间更新位姿状态，a为历史观测位姿状态对应的第一雅可比矩阵，xk-1|k-1为历史预测位姿状态，b为当前控制量对应的第二雅可比矩阵，uk为当前控制量。

(3)将第一雅可比矩阵、第二雅可比矩阵、历史预测协方差矩阵及自定义代数量代入第四预设算法中，计算时间更新协方差矩阵。

其中，自定义代数量为自行定义的、上一时刻无人驾驶汽车对应的代数量。

在本发明实施例中，无人驾驶汽车在根据第一雅可比矩阵、第二雅可比矩阵、当前控制量及历史预测位姿状态，计算得到自身对应的时间更新位姿状态后，便可基于第一雅可比矩阵和第二雅可比矩阵对历史预测协方差矩阵进行时间更新处理，从而获得自身对应的时间更新协方差矩阵，即将第一雅可比矩阵、第二雅可比矩阵、历史预测协方差矩阵及自定义代数量代入第四预设算法中，从而计算时间更新协方差矩阵，其中，第四预设算法具体如下：

pk|k-1＝apk-1|k-1a^t+bqk-1b^t

其中，pk|k-1为时间更新协方差矩阵，a为历史观测位姿状态对应的第一雅可比矩阵，pk-1|k-1为历史预测协方差矩阵，b为当前控制量对应的第二雅可比矩阵，qk-1为自定义代数量。

203、获取当前观测定位结果。

在本发明实施例中，无人驾驶汽车在根据自身对应的当前控制量、历史观测位姿状态、历史预测位姿状态及历史预测协方差矩阵，计算得到自身对应的时间更新位姿状态及时间更新协方差矩阵后，便可获取自身对应的当前观测定位结果，以便后续基于自身对应的当前观测定位结果，计算自身对应的当前预测位姿状态。以下将对无人驾驶汽车如何获取当前观测定位结果进行详细说明。

(1)无人驾驶汽车将时间更新位姿状态代入第五预设算法中，从而计算当前预测定位结果，其中，第五预设算法具体如下：

zk|k-1＝h(xk|k-1)

其中，zk|k-1为当前预测定位结果，xk|k-1为时间更新位姿状态，h为单位矩阵。

(2)无人驾驶汽车获取第一当前定位结果，并计算第一当前定位结果与当前预测定位结果之间的第一差值，即第一差值＝第一当前定位结果-当前预测定位结果。

(3)当无人驾驶汽车判定第一差值小于或等于预设阈值向量时，无人驾驶汽车将第一当前定位结果确定为当前观测定位结果。

(4)当无人驾驶汽车判定第一差值大于预设阈值向量时，无人驾驶汽车获取第二当前定位结果，并计算第二当前定位结果与当前预测定位结果之间的第二差值，即第二差值＝第二当前定位结果-当前预测定位结果。

(5)当无人驾驶汽车判定第二差值小于或等于预设阈值向量时，无人驾驶汽车将第二当前定位结果确定为当前观测定位结果。

(6)当无人驾驶汽车判定第二差值大于预设阈值向量时，无人驾驶汽车获取第三当前定位结果，并计算第三当前定位结果与当前预测定位结果之间的第三差值，即第三差值＝第三当前定位结果-当前预测定位结果。

(7)当无人驾驶汽车判定第三差值小于或等于预设阈值向量时，无人驾驶汽车将第三当前定位结果确定为当前观测定位结果。

(8)当无人驾驶汽车判定第三差值大于预设阈值向量时，无人驾驶汽车将第一差值、第二差值及第三差值中的最小值对应的当前定位结果确定为当前观测定位结果。

需要进行说明的是，第一当前定位结果为当前时刻无人驾驶汽车基于差分gps定位法、激光定位法及视觉定位法获取得到的第一个定位结果，第二当前定位结果为当前时刻无人驾驶汽车基于差分gps定位法、激光定位法及视觉定位法获取得到的第二个定位结果，第三当前定位结果为当前时刻无人驾驶汽车基于差分gps定位法、激光定位法及视觉定位法获取得到的第三个定位结果，例如，当前时刻无人驾驶汽车基于差分gps定位法获取得到第一个定位结果、基于激光定位法获取得到第二个定位结果、基于视觉定位法获取得到第三个定位结果，则基于差分gps定位法获取得到的定位结果为第一当前定位结果，基于激光定位法获取得到的定位结果为第二当前定位结果，基于视觉定位法获取得到的定位结果为第三当前定位结果。

204、获取当前观测矩阵及当前观测协方差矩阵，并将当前观测矩阵、当前观测协方差矩阵及时间更新协方差矩阵代入第一预设算法中，计算当前卡尔曼增益矩阵。

其中，关于步骤204、获取当前观测矩阵及当前观测协方差矩阵，并将当前观测矩阵、当前观测协方差矩阵及时间更新协方差矩阵代入第一预设算法中，计算当前卡尔曼增益矩阵，可以参考图1对应部分的描述，本发明实施例此处将不再赘述。

205、将时间更新位姿状态、当前卡尔曼增益矩阵及当前观测定位结果代入第二预设算法中，计算当前预测位姿状态。

其中，关于步骤205、将时间更新位姿状态、当前卡尔曼增益矩阵及当前观测定位结果代入第二预设算法中，计算当前预测位姿状态，可以参考图1对应部分的描述，本发明实施例此处将不再赘述。

进一步的，在本发明实施例中，无人驾驶汽车在根据自身对应的时间更新位姿状态、当前卡尔曼增益矩阵及当前观测定位结果，计算得到自身对应的当前预测位姿状态后，可以将在步骤204中获取得到的当前观测矩阵、在步骤204中计算得到的当前卡尔曼增益矩阵及在步骤202中计算得到的时间更新协方差矩阵代入第六预设算法中，从而计算自身对应的当前预测协方差矩阵，并将计算得到的当前预测协方差矩阵、在步骤205中计算得到的当前预测位姿状态及在步骤203中获取得到的当前观测定位结果缓存至本地存储空间中，以便无人驾驶汽车在下一时刻进行预测无人驾驶汽车位姿状态操作的过程中，将本次操作缓存的当前预测协方差矩阵作为历史预测协方差矩阵、将当前预测位姿状态作为历史预测位姿状态，以及将当前观测定位结果作为历史当前观测定位结果，来计算下一时刻无人驾驶汽车对应的预测位姿状态，其中，第六预设算法具体如下：

pk|k＝(i-kkhk)pk|k-1

其中，pk|k为当前预测协方差矩阵，i为元素全为1的矩阵，kk为当前卡尔曼增益矩阵，hk为当前观测矩阵，pk|k-1为时间更新协方差矩阵。

206、将当前预测位姿状态进行输出显示。

在本发明实施例中，无人驾驶汽车在根据自身对应的时间更新位姿状态、当前卡尔曼增益矩阵及当前观测定位结果，计算得到自身对应的当前预测位姿状态后，便可将计算得到的当前预测位姿状态进行输出显示，以便工作人员获知当前时刻无人驾驶汽车对应的朝向、位置及速度。

为了实现上述目的，根据本发明的另一方面，本发明实施例还提供了一种存储介质，所述存储介质包括存储的程序，其中，在所述程序运行时控制所述存储介质所在设备执行上述所述的无人驾驶汽车的定位方法。

为了实现上述目的，根据本发明的另一方面，本发明实施例还提供了一种处理器，所述处理器用于运行程序，其中，所述程序运行时执行上述所述的无人驾驶汽车的定位方法。

进一步的，作为对上述图1及图2所示方法的实现，本发明另一实施例还提供了一种无人驾驶汽车的定位装置。该装置实施例与前述方法实施例对应，为便于阅读，本装置实施例不再对前述方法实施例中的细节内容进行逐一赘述，但应当明确，本实施例中的装置能够对应实现前述方法实施例中的全部内容。该装置应用于精准的对无人驾驶汽车的位姿进行定位，从而获取得到准确性较高的定位结果，具体如图3所示，该装置包括：

第一获取单元31，用于获取当前控制量、历史观测位姿状态、历史预测位姿状态及历史预测协方差矩阵；

第一计算单元32，用于根据所述当前控制量、所述历史观测位姿状态、所述历史预测位姿状态及所述历史预测协方差矩阵，计算时间更新位姿状态及时间更新协方差矩阵；

第二获取单元33，用于获取当前观测定位结果；

第三获取单元34，用于获取当前观测矩阵及当前观测协方差矩阵；

第二计算单元35，用于将所述当前观测矩阵、所述当前观测协方差矩阵及所述时间更新协方差矩阵代入第一预设算法中，计算所述当前卡尔曼增益矩阵；

第三计算单元36，用于将所述时间更新位姿状态、所述当前卡尔曼增益矩阵及所述当前观测定位结果代入第二预设算法中，计算当前预测位姿状态。

进一步的，如图4所示，第一计算单元32包括：

第一偏导模块321，用于使用预设函数对所述历史观测位姿状态进行偏导处理，以获得所述历史观测位姿状态对应的第一雅可比矩阵；

第二偏导模块322，用于使用所述预设函数对所述当前控制量进行偏导处理，以获得所述当前控制量对应的第二雅可比矩阵；

第一计算模块323，用于将所述第一雅可比矩阵、所述第二雅可比矩阵、所述当前控制量及所述历史预测位姿状态代入第三预设算法中，计算所述时间更新位姿状态；

第二计算模块324，用于将所述第一雅可比矩阵、所述第二雅可比矩阵、所述历史预测协方差矩阵及自定义代数量代入第四预设算法中，计算所述时间更新协方差矩阵。

进一步的，如图4所示，第二获取单元33包括：

第三计算模块331，用于将所述时间更新位姿状态代入第五预设算法中，计算当前预测定位结果；

第四计算模块332，用于获取第一当前定位结果，并计算所述第一当前定位结果与所述当前预测定位结果之间的第一差值；

第一确定模块333，用于当所述第一差值小于或等于预设阈值向量时，将所述第一当前定位结果确定为所述当前观测定位结果；

第五计算模块334，用于当所述第一差值大于所述预设阈值向量时，获取第二当前定位结果，并计算所述第二当前定位结果与所述当前预测定位结果之间的第二差值；

第二确定模块335，用于当所述第二差值小于或等于所述预设阈值向量时，将所述第二当前定位结果确定为所述当前观测定位结果；

第六计算模块336，用于当所述第二差值大于所述预设阈值向量时，获取第三当前定位结果，并计算所述第三当前定位结果与所述当前预测定位结果之间的第三差值；

第三确定模块337，用于当所述第三差值小于或等于所述预设阈值向量时，将所述第三当前定位结果确定为所述当前观测定位结果；

第四确定模块338，用于当所述第三差值大于所述预设阈值向量时，将所述第一差值、第二差值及第三差值中的最小值对应的当前定位结果确定为所述当前观测定位结果；其中，所述第一当前定位结果为基于差分gps定位法、激光定位法及视觉定位法获取得到的第一个定位结果，所述第二当前定位结果为基于差分gps定位法、激光定位法及视觉定位法获取得到的第二个定位结果，所述第三当前定位结果为基于差分gps定位法、激光定位法及视觉定位法获取得到的第三个定位结果。

进一步的，如图4所示，该装置还包括：

第四计算单元37，用于在第三计算单元36将所述时间更新位姿状态、所述当前卡尔曼增益矩阵及所述当前观测定位结果代入第二预设算法中，计算当前预测位姿状态之后，将所述当前观测矩阵、所述当前卡尔曼增益矩阵及所述时间更新协方差矩阵代入第六预设算法中，计算当前预测协方差矩阵；

缓存单元38，用于将所述当前预测位姿状态、所述当前预测协方差矩阵及所述当前观测定位结果缓存至本地存储空间中。

进一步的，如图4所示，该装置还包括：

输出单元39，用于在第三计算单元36将所述时间更新位姿状态、所述当前卡尔曼增益矩阵及所述当前观测定位结果代入第二预设算法中，计算当前预测位姿状态之后，将所述当前预测位姿状态进行输出显示。

本发明实施例提供的一种无人驾驶汽车的定位方法及装置，与现有技术中无人驾驶汽车基于差分gps定位法、激光定位法及视觉定位法三种定位方法中的任意一种定位方法获取自身对应的定位结果相比，本发明实施例能够使无人驾驶汽车先获取自身对应的当前控制量、历史观测位姿状态、历史预测位姿状态及历史预测协方差矩阵，再根据获取的当前控制量、历史观测位姿状态、历史预测位姿状态及历史预测协方差矩阵，计算自身对应的时间更新位姿状态及时间更新协方差矩阵，并在获取得到自身对应的当前观测定位结果后，获取自身对应的当前观测矩阵及当前观测协方差矩阵，并根据获取的当前观测矩阵、当前观测协方差矩阵及时间更新协方差矩阵，计算自身对应的当前卡尔曼增益矩阵，以及根据计算获得的当前卡尔曼增益矩阵、时间更新位姿状态及当前观测定位结果，计算自身对应的当前预测位姿状态。由于，无人驾驶汽车是基于自身对应的当前观测定位结果、当前卡尔曼增益矩阵以及时间更新位姿状态，计算自身对应的当前预测位姿状态，即基于ekf(extendedkalmanfilter,扩展卡尔曼滤波器)框架预测当前时刻自身对应的定位结果，因此，无人驾驶汽车在复杂的港口环境内也能够精准的对自身的位姿进行定位，从而能够保证无人驾驶汽车在作业过程中的安全性和工作稳定性。

所述无人驾驶汽车的定位装置包括处理器和存储器，上述第一获取单元、第一计算单元、第二获取单元、第三获取单元、第二计算单元和第三计算单元等均作为程序单元存储在存储器中，由处理器执行存储在存储器中的上述程序单元来实现相应的功能。

处理器中包含内核，由内核去存储器中调取相应的程序单元。内核可以设置一个或以上，通过调整内核参数来精准的对无人驾驶汽车的位姿进行定位，从而获取得到准确性较高的定位结果。

存储器可能包括计算机可读介质中的非永久性存储器，随机存取存储器(ram)和/或非易失性内存等形式，如只读存储器(rom)或闪存(flashram)，存储器包括至少一个存储芯片。

本发明实施例提供了一种存储介质，其上存储有程序，该程序被处理器执行时实现以上实施例中任一项所述的无人驾驶汽车的定位方法。

本发明实施例提供了一种处理器，所述处理器用于运行程序，其中，所述程序运行时执行以上实施例中任一项所述的无人驾驶汽车的定位方法。

本发明实施例提供了一种设备，设备包括处理器、存储器及存储在存储器上并可在处理器上运行的程序，处理器执行程序时实现以下步骤：

获取当前控制量、历史观测位姿状态、历史预测位姿状态及历史预测协方差矩阵，并根据所述当前控制量、所述历史观测位姿状态、所述历史预测位姿状态及所述历史预测协方差矩阵，计算时间更新位姿状态及时间更新协方差矩阵；

获取当前观测定位结果；

获取当前观测矩阵及当前观测协方差矩阵，并将所述当前观测矩阵、所述当前观测协方差矩阵及所述时间更新协方差矩阵代入第一预设算法中，计算所述当前卡尔曼增益矩阵；

将所述时间更新位姿状态、所述当前卡尔曼增益矩阵及所述当前观测定位结果代入第二预设算法中，计算当前预测位姿状态。

进一步的，所述根据所述当前控制量、所述历史观测位姿状态、所述历史预测位姿状态及所述历史预测协方差矩阵，计算时间更新位姿状态及时间更新协方差矩阵，包括：

使用预设函数对所述历史观测位姿状态进行偏导处理，以获得所述历史观测位姿状态对应的第一雅可比矩阵；

使用所述预设函数对所述当前控制量进行偏导处理，以获得所述当前控制量对应的第二雅可比矩阵；

将所述第一雅可比矩阵、所述第二雅可比矩阵、所述当前控制量及所述历史预测位姿状态代入第三预设算法中，计算所述时间更新位姿状态；

将所述第一雅可比矩阵、所述第二雅可比矩阵、所述历史预测协方差矩阵及自定义代数量代入第四预设算法中，计算所述时间更新协方差矩阵。

进一步的，所述获取当前观测定位结果，包括：

将所述时间更新位姿状态代入第五预设算法中，计算当前预测定位结果；

获取第一当前定位结果，并计算所述第一当前定位结果与所述当前预测定位结果之间的第一差值；

若所述第一差值小于或等于预设阈值向量，则将所述第一当前定位结果确定为所述当前观测定位结果；

若所述第一差值大于所述预设阈值向量，则获取第二当前定位结果，并计算所述第二当前定位结果与所述当前预测定位结果之间的第二差值；

若所述第二差值小于或等于所述预设阈值向量，则将所述第二当前定位结果确定为所述当前观测定位结果；

若所述第二差值大于所述预设阈值向量，则获取第三当前定位结果，并计算所述第三当前定位结果与所述当前预测定位结果之间的第三差值；

若所述第三差值小于或等于所述预设阈值向量，则将所述第三当前定位结果确定为所述当前观测定位结果；

若所述第三差值大于所述预设阈值向量，则将所述第一差值、第二差值及第三差值中的最小值对应的当前定位结果确定为所述当前观测定位结果；其中，所述第一当前定位结果为基于差分gps定位法、激光定位法及视觉定位法获取得到的第一个定位结果，所述第二当前定位结果为基于差分gps定位法、激光定位法及视觉定位法获取得到的第二个定位结果，所述第三当前定位结果为基于差分gps定位法、激光定位法及视觉定位法获取得到的第三个定位结果。

进一步的，在所述将所述时间更新位姿状态、所述当前卡尔曼增益矩阵及所述当前观测定位结果代入第二预设算法中，计算当前预测位姿状态之后，所述方法还包括：

将所述当前观测矩阵、所述当前卡尔曼增益矩阵及所述时间更新协方差矩阵代入第六预设算法中，计算当前预测协方差矩阵；

将所述当前预测位姿状态、所述当前预测协方差矩阵及所述当前观测定位结果缓存至本地存储空间中。

进一步的，在所述将所述时间更新位姿状态、所述当前卡尔曼增益矩阵及所述当前观测定位结果代入第二预设算法中，计算当前预测位姿状态之后，所述方法还包括：

将所述当前预测位姿状态进行输出显示。

本申请还提供了一种计算机程序产品，当在数据处理设备上执行时，适于执行初始化有如下方法步骤的程序代码：获取当前控制量、历史观测位姿状态、历史预测位姿状态及历史预测协方差矩阵，并根据所述当前控制量、所述历史观测位姿状态、所述历史预测位姿状态及所述历史预测协方差矩阵，计算时间更新位姿状态及时间更新协方差矩阵；获取当前观测定位结果；获取当前观测矩阵及当前观测协方差矩阵，并将所述当前观测矩阵、所述当前观测协方差矩阵及所述时间更新协方差矩阵代入第一预设算法中，计算所述当前卡尔曼增益矩阵；将所述时间更新位姿状态、所述当前卡尔曼增益矩阵及所述当前观测定位结果代入第二预设算法中，计算当前预测位姿状态。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

在一个典型的配置中，计算设备包括一个或多个处理器(cpu)、输入/输出接口、网络接口和内存。

存储器可能包括计算机可读介质中的非永久性存储器，随机存取存储器(ram)和/或非易失性内存等形式，如只读存储器(rom)或闪存(flashram)。存储器是计算机可读介质的示例。

计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(pram)、静态随机存取存储器(sram)、动态随机存取存储器(dram)、其他类型的随机存取存储器(ram)、只读存储器(rom)、电可擦除可编程只读存储器(eeprom)、快闪记忆体或其他内存技术、只读光盘只读存储器(cd-rom)、数字多功能光盘(dvd)或其他光学存储、磁盒式磁带，磁带磁磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括暂存电脑可读媒体(transitorymedia)，如调制的数据信号和载波。

还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括要素的过程、方法、商品或者设备中还存在另外的相同要素。

本领域技术人员应明白，本申请的实施例可提供为方法、系统或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。

以上仅为本申请的实施例而已，并不用于限制本申请。对于本领域技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本申请的权利要求范围之内。