[发明专利]自动导引运输车定位方法、定位系统及自动导引运输系统

权利要求书

1.一种自动导引运输车的定位方法，其特征在于，所述定位方法包括：

根据目标自动导引运输车的轴距、导航仪的位姿角、驱动轮转向角和驱动轮转速确定所述目标自动导引运输车的状态预测模型，其中，所述目标自动导引运输车包括所述导航仪和所述驱动轮，所述状态预测模型的状态向量包括驱动轮位置和导航仪位姿角；

获取第k-1个采样周期的所述驱动轮的位置坐标、第k-1个采样周期的位姿角、第k个采样周期的驱动轮实测转速、第k个采样周期的驱动轮转向角；

将第k-1个采样周期的所述驱动轮的位置坐标、第k-1个采样周期的位姿角、第k个采样周期的驱动轮实测转速、第k个采样周期的驱动轮转向角输入所述状态预测模型，获得第k个采样周期的状态向量预测值；

判断所述导航仪的最新采样时间是否小于驱动轮传感器的最新采样时间，获得判断结果；

当所述判断结果表示所述导航仪的最新采样时间小于所述驱动轮传感器的最新采样时间时，则根据所述状态向量预测值、所述导航仪与所述驱动轮的距离确定所述导航仪的期望位置；

所述根据所述状态向量预测值、所述导航仪与所述驱动轮的距离确定所述导航仪的期望位置，具体包括：

获取所述导航仪与所述驱动轮的距离和所述导航仪与所述驱动轮的位置变换关系式，所述位置变换关系式为：

其中，(x_n,y_n)表示导航仪的位置坐标，(x_d,y_d)表示驱动轮的位置坐标，h表示导航仪与驱动轮的距离，ψ为位姿角；

对所述位置变换关系式进行逆变换，获得导航仪位置预测模型；

将所述状态向量预测值、所述导航仪与所述驱动轮的距离输入所述导航仪位置预测模型，获得所述导航仪的期望位置；

当所述判断结果表示所述导航仪的最新采样时间大于或者等于所述驱动轮传感器的最新采样时间时，根据所述状态向量预测值、第k个采样周期的位姿角、第k个采样周期的导航仪实测位置确定所述导航仪的期望位置。

2.根据权利要求1所述的定位方法，其特征在于，所述状态预测模型为：

其中，表示第k个采样周期的状态向量预测值，表示驱动轮的横坐标预测值，表示驱动轮的纵坐标预测值，表示导航仪位姿角预测值，Δt表示驱动轮传感器的采样周期，v表示驱动轮的实测转速，表示驱动轮的加速度估值，θ_k表示驱动轮转向角，L为驱动轮与后轮中点距离。

3.根据权利要求2所述的定位方法，其特征在于，所述根据所述状态向量预测值、第k个采样周期的位姿角、第k个采样周期的导航仪实测位置确定所述导航仪的期望位置，具体包括：

获取所述导航仪与所述驱动轮的距离、观测矩阵和所述导航仪与所述驱动轮的位置变换关系式，所述位置变换关系式为：

其中，(x_n,y_n)表示导航仪的位置坐标，(x_d,y_d)表示驱动轮的位置坐标，h表示导航仪与驱动轮的距离，ψ为位姿角；

对所述位置变换关系式进行逆变换，获得导航仪位置预测模型；

将所述状态向量预测值、所述导航仪与所述驱动轮的距离输入所述导航仪位置预测模型，获得导航仪预测位置；

根据所述导航仪预测位置和第k个采样周期的导航仪实测位置确定预测误差；

根据所述状态预测模型确定所述状态预测模型的状态协方差矩阵；

根据所述状态协方差矩阵确定卡尔曼增益；

根据所述预测误差、所述卡尔曼增益和所述所述导航仪的预测位置确定所述导航仪的期望位置。