[发明专利]基于通信传感异步数据融合的协作邻居车辆定位方法

权利要求书

1.一种基于通信传感异步数据融合的协作邻居车辆定位方法，包括如下步骤：

(1)周期性地测量自身及邻居车辆包括位置、速度在内的运动状态信息，将自身状态信息通过V2V广播给所有邻居车辆，同时接收邻居车辆通信广播的状态测量信息；

若将自身车辆记做i，其任一邻居车辆记做j，则车辆i和j在t时刻的运动状态表达如下：

其中：S_i(t)和S_j(t)分别为车辆i和j在t时刻的运动状态，和分别为车辆i和j在t时刻的横向位置，和分别为车辆i和j在t时刻的纵向位置，v_i(t)和v_j(t)分别为车辆i和j在t时刻的速度，θ_i(t)和θ_j(t)分别为车辆i和j在t时刻行驶方向与正东方向的夹角，T表示转置，t为非负实数；

若将自身车辆i获得邻居车辆j的第k个状态测量信息表示为k为自然数，r_k∈{s,g}，当r_k＝s则表示该信息由车辆i的传感器测量得到且S_j,k＝S_j(t_k)，S_j(t_k)为车辆j对应t_k时刻的运动状态，为车辆i自身的状态测量误差，为车辆i对车辆j的传感测量误差；当r_k＝g则表示该信息由车辆i通信接收获得且为车辆j自身的状态测量误差；

(2)从获得的邻居车辆信息中提取车辆ID、信息采集时间以及位置和速度，并根据邻居车辆ID和信息采集时间来判断是否需要为该邻居车辆新建滤波进程；对于t_k时刻获得邻居车辆j的第k个状态测量信息根据邻居车辆j的ID判断是否为新的邻居车辆，若是则为该邻居车辆新建一个滤波进程，若否则说明自身车辆i中已有该邻居车辆的滤波进程，进而根据获得的其上一状态测量信息采集时间t_k-1来判断的时效性：若t_k-t_k-1＞aT，则说明间隔时间较长，删除原有滤波进程并为该邻居车辆新建一个滤波进程，否则在原有滤波进程上继续处理，T为传感器测量周期，a为预设的时效性参数且为正整数；

(3)采用通信传感异步数据融合算法对邻居车辆进行定位估计，即根据滤波进程中上一时刻粒子状态预测当前时刻粒子状态，然后利用获得的邻居车辆状态测量信息进行粒子权重更新，最后完成邻居车辆运动状态估计并对粒子进行重采样；

所述通信传感异步数据融合算法对于需在车辆i中新建滤波进程的邻居车辆j即k＝0情况下，当r_k＝s时，S_j,0满足均值为协方差矩阵为的高斯分布；当r_k＝g时，S_j,0满足均值为协方差矩阵为的高斯分布；进而在已知S_j,0的分布后，从该分布中随机采样得到每一粒子状态每一粒子权重设为m＝1,2,...,M，M为粒子数量；

所述协方差矩阵的分别设置如下：

其中：和分别为车辆i通过GPS测量得到关于自身横向位置、纵向位置、速度和角度的噪声方差，和分别为车辆j通过GPS测量得到关于自身横向位置、纵向位置、速度和角度的噪声方差，和分别为车辆i通过传感器测量得到关于车辆j横向相对位置、纵向相对位置、相对速度和相对角度的噪声方差；

所述通信传感异步数据融合算法对于在车辆i中已有滤波进程的邻居车辆j即k≠0情况下，根据滤波进程中上一时刻粒子状态预测当前时刻粒子状态的方法为：若r_k-1＝r_k即t_k-1时刻和t_k时刻邻居车辆j的状态测量信息为车辆i通过同一方式得到，满足t_k-t_k-1＝bT且b为小于a的正整数；对于每一粒子，从噪声分布中随机采样得到车辆j在t_k-1时刻的运动速度噪声和运动角度噪声进而结合t_k-1时刻的每一粒子状态和离散非线性动力模型预测得到t_k-1+T时刻车辆j的运动状态S_j(t_k-1+T)，以此递推最终得到t_k时刻的每一粒子状态m＝1,2,...,M，M为粒子数量；

若r_k-1≠r_k即t_k-1时刻和t_k时刻邻居车辆j的状态测量信息为车辆i通过不同方式得到，满足t_k-t_k-1＝bT+τ，τ为车辆i和j的信息采集时间差且τ＜T；对于每一粒子，根据t_k-1时刻的每一粒子状态和离散非线性动力模型逐步预测得到t_k-1+bT时刻和t_k-1+(b+1)T时刻车辆j的运动状态S_j(t_k-1+bT)和S_j(t_k-1+(b+1)T)，则t_k时刻每一粒子状态通过以下平滑算法得到：

其中：α＝τ/T；所述离散非线性动力模型表达式如下：

其中：为车辆j在t+T时刻的横向位置，为车辆j在t+T时刻的纵向位置，v_j(t+T)为车辆j在t+T时刻的速度，θ_j(t+T)为车辆j在t+T时刻行驶方向与正东方向的夹角，和分别为车辆j在t时刻的运动速度噪声和运动角度噪声；

利用获得的邻居车辆状态测量信息进行粒子权重更新的方法为：对于每一粒子，其粒子权重通过以下公式进行更新；

其中：和分别为t_k-1时刻和t_k时刻的每一粒子权重，当r_k＝s时，为的概率密度函数在处的值；当r_k＝g时，为的概率密度函数在处的值；

通过以下公式计算出邻居车辆j的运动状态估计值；

其中：为t_k时刻邻居车辆j的运动状态估计值；

对粒子进行重采样的方法为：当有效粒子数N_eff小于预设的阈值N_th时进行粒子重采样且首先将(0,1]的粒子权重取值区间分成M个子区间，每个子区间为(λ_m-1,λ_m]且然后随机产生M个在[0,1]上均匀分布的值u_l，l＝1,2,...,M，当u_l落在区间(λ_m-1,λ_m]时，则将作为对应新的粒子状态即新的粒子权重设为1/M。