[发明专利]一种定位方法及装置

权利要求书

1.一种定位方法，其特征在于，包括：

根据待定位的目标对象上一时刻的目标运行状态，预测所述目标对象当前时刻的预测运行状态，其中，所述预测运行状态中至少包括所述目标对象的预测位置信息；

根据所述预测运行状态所对应的预测位置信息，从地图数据中获取包含所述预测位置信息的预设范围内的参数化曲线描述信息，所述参数化曲线描述信息包括所述地图数据中的道路几何信息和道路类型；

根据获取到的参数化曲线描述信息，获得所述目标对象当前时刻对应的道路类型和道路中心线几何方程；

根据所述道路类型和道路中心线几何方程，分别确定当前时刻的运行状态观测量和运行状态观测值，其中，所述运行状态观测量表示能够获取到的状态变量，所述运行状态观测值为所述运行状态观测量对应的约束取值；

分别将所述当前时刻的运行状态观测量对所述各状态变量进行一阶求导，获得观测雅可比矩阵，并确定观测协方差矩阵，其中，所述观测协方差矩阵表征所述运行状态观测值的观测置信度；

将所述观测雅可比矩阵和所述观测协方差矩阵，以及所述当前时刻的运行状态观测量和所述运行状态观测值作为所述目标对象的第一运行轨迹约束条件，并根据所述第一运行轨迹约束条件，对所述预测运行状态进行更新，获得所述目标对象当前时刻的目标运行状态。

2.如权利要求1所述的方法，其特征在于，所述目标运行状态中至少包括所述目标对象的目标位置信息和目标姿态信息，所述预测运行状态中还至少包括所述目标对象的预测姿态信息；

所述目标运行状态和所述预测运行状态均由状态变量集合表示，所述状态变量集合中每个状态变量对应一个状态更新方程，各状态更新方程组合为状态更新方程组，并所述状态更新方程组的因变量为当前时刻的目标运行状态中各状态变量，自变量为上一个时刻的目标运行状态中各状态变量以及输入向量，所述输入向量包括表示加速度和角速度的各输入变量；

则根据待定位的目标对象上一时刻的目标运行状态，预测所述目标对象当前时刻的预测运行状态，包括：

获取测量的各输入变量和所述目标对象上一时刻的目标状态协方差矩阵；

基于扩展卡尔曼滤波方法，分别将所述各状态更新方程对各状态变量进行一阶求导，获得状态转移雅可比矩阵，并分别将所述各状态更新方程对所述各输入变量进行一阶求导，获得输入雅可比矩阵；

根据所述状态转移雅可比矩阵、所述输入雅可比矩阵、所述上一时刻的目标运行状态，以及获取的各输入变量，预测所述目标对象当前时刻的预测运行状态；

根据所述状态转移雅可比矩阵、所述输入雅可比矩阵、所述上一时刻的目标状态协方差矩阵，以及预设的测量噪声协方差矩阵和状态转移过程噪声协方差矩阵，预测所述目标对象当前时刻的预测状态协方差矩阵。

3.如权利要求1所述的方法，其特征在于，根据所述道路类型和道路中心线几何方程，分别确定当前时刻的运行状态观测量和运行状态观测值，具体包括：

若所述道路类型为直线路段，道路中心线几何方程为ax+by+c＝0，则分别确定当前时刻的运行状态观测量为[c-ax_k-by_k；theta_k]，运行状态观测值为[0；atan(a/b)]，其中，x_k表示当前时刻的正东方向位置，y_k表示当前时刻的正北方向位置，theta_k表示当前时刻的航向角。