[发明专利]一种针对可控智能载具的位置估计方法

权利要求书

1.一种针对可控智能载具的位置估计方法，其特征在于，包括如下步骤：

1)首先进行初始化设置：

1.1)设置状态向量s＝[x,v_x,y,v_y],其中x，y代表x轴和y轴位置，v_x，v_y代表x轴线速度和y轴线速度；

1.2)在每一个更新时间间隔内根据控制指令近似考虑两种状态转移方程；

匀速直线运动状态转移方程：

匀速圆周运动状态转移方程：

其中ω表示智能载具在转向时智能载具控制指令输入的角速度；

1.3)观测向量h＝[x_GPS,y_GPS,x_AOA,y_AOA]，其中x_GPS,y_GPS由GPS定位信号直接观测得到，x_AOA,y_AOA根据参考基站的位置信息和对应到达角角度信息每个基站按照y＝tan(θ_AOA,k)(x_k-x)+y_k建立目标位置直线方程，其中θ_AOA,k代表到达角度数，x_k和y_k代表基站坐标信息，x和y代表目标坐标信息；联立两个基站的目标位置直线方程，求解交点可以得到到达角估计的目标位置信息；考虑到多基站的情况下，按照min(||θ_AOA,k-θ_AOA,i|-90|),k≠i，其中θ_AOA,k代表根据接收到的来自不同基站的信号测量所得到达交角度，选取到达角角度差最接近90度的两个基站作为参考基站；观测矩阵其中每一列与状态向量中各变量对应，每一行和观测向量中各变量对应；

1.4)观测量为GPS定位坐标，GPS设备的定位误差标准差为ε_GPSm，GPS观测噪声标准差向量n_GPS＝[ε_GPS,ε_GPS]，单位为m，可以得到观测噪声协方差矩阵为到达角测量误差标准差为ε_AOA，根据公式，按照归一化定位精度表达式其中根据α_P和β_P近似采用各基站观测角度代替；R＝[R_GPS,R_AOA]^T；

1.5)系统过程噪声向量为n_Q＝[x_Q,v_xQ,y_Q,v_yQ]，其中x_Q和y_Q代表位置过程转移噪声标准差，v_xQ和v_yQ代表x方向和y方向速度过程转移噪声标准差；可以得到系统过程噪声协方差矩阵

1.6)设置初始状态转移协方差矩阵P为单位矩阵，维度和状态向量中的状态数量一致为4行4列；

2)步骤1)中初始化过程，在小车运动过程中综合接收到的GPS信号，到达角测量信息和控制信息进行位置估计滤波的状态更新迭代，得到更精确的位置估计信息，具体步骤如下：

2.1)GPS接收模块接收GPS卫星定位信号，控制指令接收模块接收来自基站发送的控制指令，同时到达角测量模块测量各基站信号的到达角信息；

2.2)控制指令包含两种形式：控制指令0：智能载具以速度v做匀速直线运动；控制指令1：智能载具以角速度ωrad/s做匀速圆周运动；

2.3)将GPS卫星定位信号，控制指令和到达角信息传入导航处理模块进行位置估计；

2.4)智能载具控制指令接收时间间隔为ΔT，位置估计算法更新间隔和智能载具控制指令接收时间间隔一致；

2.5)进行时间更新，具体包括如下步骤：

2.5.1)根据上一时刻后验预测协方差矩阵P_k|k，对P_k|k进行SVD分解，后续表达中下标k代表上一时刻，下标k+1代表当前时刻，初始时刻从k＝1开始，初始时刻设置为单位矩阵；

其中S_k为奇异值矩阵对角线元素为奇异值，剩余元素为0，U_k和V_k为矩阵P_k|k分解得到的左奇异矩阵和右奇异矩阵。

2.5.2)容积点传播计算：

X_i,k|k＝U_kS_kξ_iω_i+x_k|k

其中容积点传播后矩阵X_i,k|k为第i列，n代表状态数量；ξ_i代表容积点采样矩阵中的第i列，容积点采样矩阵ξ＝[I_n,-I_n]，其中I_n代表n维单位方阵；ω_i＝1/2n代表平均权值，下述所有ω_i含义和计算方式相同，x_k|k代表上一时刻后验状态向量，U_k,S_k均由上一步SVD分解得出，初始时刻状态向量结合实际情况按照1.3)步骤进行设置；

2.5.3)根据2.5.2)步骤中的得到的矩阵X_k|k,计算状态转移；

控制指令0：

控制指令1：

得到当前状态先验预测值其中F_cv和F_ct为的设置参照步骤1.2)中设置，表示状态转移方程；

2.5.4)更新预测协方差矩阵：Q为系统状态转移噪声协方差矩阵；

其中m＝2n,数值上等于步骤2.5.2)中容积点采样矩阵的列数，代表采样维数。

2.6)进行观测更新，具体包括如下步骤：

2.6.1)将时间更新中得到的预测协方差矩阵P_k+1|k进行SVD分解：

2.6.2)按照步骤2.5.2)容积点采样矩阵一致的构造方式构造容积点采样矩阵，对进行容积点传播计算：

其中i代表每个矩阵的第i列；

2.6.3)通过测量矩阵对2.6.2)步骤中得到的状态矩阵X_k+1|k和测量矩阵H做矩阵乘法，得到测量预测值；

Z_i,k+1|k＝HX_i,k+1|k

即将矩阵X_k+1|k的每一列和测量矩阵做矩阵乘法，后逐列相加得到观测状态预测向量

2.6.4)将实际测量值z_k+1和2.5.3)步骤中得到的测量预测值相减，得到残差y_k+1；

2.6.5)根据观测预测值修正观测噪声协方差矩阵P_zz,k+1；

其中R为当前时刻的观测噪声协方差矩阵，计算预测互协方差矩阵P_xz,k+1；

2.7)滤波结果计算，具体包括如下步骤：

2.7.1)计算滤波增益：

K_k+1＝P_xz,k+1/P_zz,k+1

2.7.2)利用2.5.3)步骤中得到的先验估计值和2.6.4)步骤中得到的残差y_k+1进行状态滤波更新，得到当前时刻最佳后验状态向量

2.7.3)计算后验预测协方差矩阵P_k+1|k+1；

将得到的后验状态向量和后验预测协方差矩阵P_k+1|k+1作为下一时刻的初始条件循环带入位置估计算法得到每一时刻的最佳后验状态向量，其中后验状态向量中的第一项和第三项为当前输出的定位坐标。