移动目标定位算法

实时探测目标并对目标进行实时定位。

1.移动目标计算机仿真建模

假定目标做匀速直线运动，第i个观测站的位置为(x,y)，目标运动模型可以写成如下形式:

其中：
注：Γ\GammaΓ仿真时用G表示

u(k)的均方差Q=Wdiag([1,1])，w为一个可调节的参数，w<<1。v(k)的均方差R=5.

例：在一个100mx 100m的环境用计算机模拟一目标匀速运动的情形，如式(3-32),目标受到空气阻力影响，导致运动轨迹不一:定是匀速直线的，即目标出现一定的轨迹抖动或弯曲。假定目标的初始位置为(0,0)、速度为(0.8,0.6), 过程噪声方差可以设置不同大小以观测计算机模拟的目标运动轨迹。

function TargetMotionModel %计算机模拟目标运动模型
dt=1;            %采样时间,单位:秒
Time=30;         %模拟总时长，也叫步数
X=zeros(4,Time); %目标在Time时间内各个时刻的状态初始化
x0=0;y0=0;vx=0.8;vy=0.6; %目标初始状态，包括位置和速度
X(:,1)=[x0,vx,y0,vy]'; %第一个时刻的状态,也叫初始状态
%系统信息，系统不同,状态转移矩阵和过程噪声驱动矩阵也不同
F=[1 dt 0 0;0 1 0 0;0 0 1 dt;0 0 0 1]; %公式(3-32)中的φ
G=[dt^2/2 0;dt 0;0 dt^2/2;0 dt]; %公式(3-32)中的r
Q=0.1; %公式(3-32)中u(k)的方差,过程噪声
u=sqrt(Q)*randn(2,Time) ;
%现在开始模拟目标随着时间推移,开始发生位置移动
for k= 2:TimeX(:,k)=F*X(:,k-1)+G*u(:,k);
end
figure
hold on;box on;%设置坐标轴的展示形式
plot(X(1,:),X(3,:),'-r.');%画出目标的运动轨迹

运行结果：

从左到右分别为，Q=0.00001时，目标基本匀速直线运动；Q=0.001时，目标的轨迹已经开始弯曲；Q=0.1时，已经无法相信目标是在做匀速直线运动哭了，因为这时的过程噪声非常大，导致目标的随机性也大。

2.基于距离观测的运动目标定位

假定有3个及以上的观测站对目标进行观测，第
i个观测站的测量方程为:

最后把这些观测站测量的距离信息传给融合中心，融合中心用最小二乘法就能实现对移动目标的动态定位。

例：在二维平面空间里，假定目标做匀速直线运动，目标运动的初始状态为x(0)=[0,1.5,20,1.4]，目标受到摩擦力的影响存在过程噪声。在环境中有5个观测站对目标距离进行探测，观测站传感器的测量噪声方差为R=1，采样时间为Is，总运行时间为60s，MATLAB仿真如下：

function TrackingByDist %基于距离的跟踪(连续定位)
%系统模型的基本信息，包括场地大小、观测站位置.模型驱动参数等
Length=100;                          %场地空间，单位：米
Width=100;                           %场地空间，单位：米
Node_number=5;                       %观测站的个数,最少必须有3个
for i=1:Node_number                  %观测站的位置初始化,这里位置是随机给定的Node(i).x= Width * rand;Node(i).y= Length * rand;Node(i).D= Node(i).x^2+Node(i).y^2; % 固定参数便于位置估计
end
T=1;                                 %雷达采样时间,单位:秒
N= 60/T;                             %总的采样次数
delta_w=1e-3;                         %如果增大这个参数,目标真实轨迹就是曲线了
Q=delta_w*diag([0.5,1]) ;             %过程噪声均值
R=1;                                 %观测噪声方差
G=[T^2/2,0;T,0;0,T^2/2;0,T];         %过程噪声驱动矩阵
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1]; %状态转移矩阵%开始模拟目标运动,观测在目标运动中同时采样,测量距离
X(:,1)=[0,1.5,20,1.4];               %目标初始位置、速度，状态参数格式[x,vx,y,vy]
Z=zeros( Node_number,N);            %各观测站的传感器对位置的观测
Xn=zeros(2,N);                       %最小二乘估计出的位置
for t=2:N                            %状态方程的建模:目标运动,时间从1T→NTX(:,t)=F*X(:,t-1)+G * sqrtm(Q) * randn(2,1); % 目标真实轨迹
end
for t=1:N                            %观测方程的建模:多观测站观测目标，时间从1T→NTfor i= 1:Node_number             %名个观测站对1时刻目标距离测量[d1,d2]=DIST(X(:,t),Node(i)); %dl是真实距离,d2是真实距离的平方Z(i,t)= d1+sqrtm(R) * randn; %模拟观测的距离受到噪声的污染end%对当前时刻目标位置的二乘法估算A=[];b=[];for i=2:Node_numberA(i-1,:)= 2*[Node(i).x-Node( 1).x,Node(i).y-Node(1).y];b(i-1,1)= [Z(1,t)^2-Z(i,t)^2+Node(i).D-Node(1).D];endXn(:,t)= inv(A'*A)*A'*b;          %得到目标当前时刻的位置
end
for t=1:N                             %计算位置偏差，即用最小二乘位置与计算机模拟的目标真实位置做差error(t)=sqrt( (X(1,t)-Xn(1,t))^2+(X(3,t)-Xn(2,t))^2);
end
%画图
figure             %轨迹图
hold on;box on;xlabel('x/m');ylabel('y/m');       %输出图形的框架
for i= 1: Node_numberh1= plot(Node(i).x, Node(i).y,'ko','MarkerFace','g','MarkerSize',10);text(Node(i).x+2,Node(i).y,[ 'Node ', num2str(i)]);
end
h2=plot(X(1,:),X(3,:),'-r'); %目标的真实位置
h3=plot(Xn(1,:),Xn(2,:),'-k.'); %目标的估计位置
legend([h1,h2,h3 ],'Observation Station','Target Postion','Estimate Postion');
figure %偏差图
hold on;box on;xlabel( 'time/s' );ylabel( 'value of the deviation');
plot( error,'-ko' ,'MarkerFace','g');%子函数,计算两点间的距离和距离平方
function [dist ,dist2]=DIST(A,B)
dist2=(A(1)-B.x)^2+(A(3)-B.y)^2; %距离的平方
dist=sqrt(dist2); %距离
end

运行结果：

作图为定位估计与真实轨迹对比，右图为定位估计与真实位置偏差对比。

注：偏差的定义为

3.纯方位角的运动目标定位

观测站测量目标与自身的方位角的目标跟踪，又称为纯方位角的目标跟踪。当有多个观测站测量目标角度时，可以用最小二乘定位算法估计目标的位置。

第i个观测站的位置为P(xi,yi)P(x_i,y_i)P(xi,yi)，目标的位置为T(x,y)T(x,y)T(x,y),则观测站测得的角度为θi\theta_iθi,
多个观测站测量，有如下关系: .

将上式转为矩阵的形式，如下：

例：假定目标的运动模型为
目标的初始状态为X(0)= [20,1.4,0,1.5]，目标的过程噪声方差Q=1∗103∗diag([0.5,1])Q= 1*10^3*diag([0.5,1])Q=1∗103∗diag([0.5,1])，有5个观测站对目标进行探测，第i个观测站的测量方程为:

测量噪声vi(k)v_i(k)vi(k)的方差R=0.1π/180，同样采样时间为T=1s,仿真时间为60s。MATLAB仿真如下:

function TrackingByAngle
% 定位初始化
Length=100;  % 场地空间，单位：米
Width=100;   % 场地空间，单位：米
Node_number=5;      % 观测站的个数，也可以设置成3个，4个，但是最少必须要3个
for i=1:Node_number % 观测站的位置初始化，这里位置是随机给定的Node(i).x=Width*rand; Node(i).y=Length*rand;
end
T=1;                      %雷达扫描周期,
N=60/T;                   %总的采样次数,也称步长
delta_w=1e-4;              % 如果增大这个参数，目标真实轨迹就是曲线了
Q=delta_w*diag([0.5,1]) ;     % 过程噪声方差
R=0.1*pi/180;             % 观测噪声方差
G=[T^2/2,0;T,0;0,T^2/2;0,T];          % 过程噪声驱动矩阵
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];  % 状态转移矩阵
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
X=zeros(4,N);           % 目标真实状态初始化
X(:,1)=[20,1.4,0,1.5];      % 目标初始位置、速度
Z=zeros(Node_number,N); % 各观测站的传感器对位置的观测
Xn=zeros(2,N);          % 估计出的位置
for t=2:N               % 目标运动X(:,t)=F*X(:,t-1)+G*sqrtm(Q)*randn(2,1);    %目标真实轨迹
end
for t=1:Nfor i=1:Node_numbercita=hfun(X(:,t),Node(i));      % 观测角度Z(i,t)=cita+sqrtm(R)*randn;     % 模拟观测的角度受到噪声的污染end
% 对当前时刻目标位置的二乘法的估算A=[];b=[];for i=1:Node_numberA(i,:)=[tan(Z(i,t)),-1];b(i,1)=[Node(i).x*tan(Z(i,t))-Node(i).y];end  inv(A'*A)*A'*bXn(:,t)=inv(A'*A)*A'*b; % 得到目标当前时刻的位置
end
for t=1:N   % 偏差分析error(t)=sqrt( (X(1,t)-Xn(1,t))^2+(X(3,t)-Xn(2,t))^2 );
end%画图
figure
hold on;box on;xlabel('x/m');ylabel('y/m'); % 输出图形的框架
for i=1:Node_numberh1=plot(Node(i).x,Node(i).y,'ko','MarkerFace','g');text(Node(i).x+2,Node(i).y,['Node ',num2str(i)]);
end
h2=plot(X(1,:),X(3,:),'-r');
h3=plot(Xn(1,:),Xn(2,:),'-k.');
legend([h1,h2,h3],'Observation Station','True Trace','MLE Ttace');
figure
hold on;box on;xlabel('time/s');ylabel('value of the deviation');
plot(error,'-ko','MarkerFace','g');% 子函数
function angle=hfun(A,B) % 角度观测方程
angle=atan( (A(3)-B.y)/(A(1)-B.x) );

运行结果：

测量噪声从上到下分别为：R=0.001π/180、R=0.01π/180、R=0.1π/180。左边为轨迹，右边为定位偏差。

由结果可知，随着测量噪声增大，目标定位结果越来越差，即传感器信息足够可靠对提高精度至关重要，定位算法只是最大限度地减小误差。

目标定位算法（四）之移动目标定位算法相关推荐

算法(四):图解狄克斯特拉算法
算法简介狄克斯特拉算法(Dijkstra )用于计算出不存在非负权重的情况下,起点到各个节点的最短距离可用于解决2类问题: 从A出发是否存在到达B的路径: 从A出发到达B的最短路径(时间最少.或者 ...
详解SVM支持向量机算法(四:坐标上升和SMO算法)
作者:RayChiu_Labloy 版权声明:著作权归作者所有,商业转载请联系作者获得授权,非商业转载请注明出处目录背景坐标上升算法定义过程举个求解的栗子基于坐标上升的SMO算法 SMO ...
java动态分区分配算法,操作系统_动态分区分配算法课程设计_java版
<操作系统_动态分区分配算法课程设计_java版>由会员分享,可在线阅读,更多相关<操作系统_动态分区分配算法课程设计_java版(13页珍藏版)>请在人人文库网上搜索. 1. ...
使用PyTorch从零开始实现YOLO-V3目标检测算法 (四)
原文:https://blog.csdn.net/u011520516/article/details/80228130 点击查看博客原文这是从零开始实现YOLO v3检测器的教程的第4部分,在上一 ...
目标检测（Object Detection）--Faster R-CNN算法
目前目标检测领域的深度学习方法主要分为两类:two stage的目标检测算法:one stage的目标检测算法.前者是先由算法生成一系列作为样本的候选框,再通过CNN进行样本分类:后者则不用产生候选框 ...
AI算法创新赛-人车目标检测竞赛总结04
队伍:AI000038 小组成员:杨志强,林松 1. 算法介绍 1.1 相关工作当前流行的目标检测算法主要分为三种,一阶段算法:SSD,FCOS,Scaled,YOLO系列等:二阶段算法:Faste ...
目标跟踪领域研究（二）跟踪算法
视觉目标跟踪(Visual Object Tracking)是计算机视觉领域的一个重要问题.尽管近年来受到了广泛研究,目标跟踪问题由于本身的高难度.高质量数据的稀少,研究热度比目标检测.语义分割等基本 ...
刚性仿射变换算法_一种视觉定位中的点集仿射变换算法的制作方法
本发明涉及相机标定技术领域: ,具体涉及一种视觉定位中的点集仿射变换算法. 背景技术: :随着工业自动化技术的推进,越来越多的产线工件组装.检测.测量等工作正在由机器人或自动化设备逐步替代,而这些技 ...
室内定位中非视距的识别和抑制算法研究综述（部分）
源自:控制与决策作者:齐小刚陈谌李芷楠针对存在非视距(non-line-of-sight, NLOS)的室内定位算法进行研究. 首先描述室内定位中的常用技术和算法(航迹推算.指纹识别 ...
多传感器融合定位十四-基于图优化的定位方法
多传感器融合定位十四-基于图优化的定位方法 1. 基于图优化的定位简介 1.1 核心思路 1.2 定位流程 2. 边缘化原理及应用 2.1 边缘化原理 2.2 从滤波角度理解边缘化 3. 基于kitt ...

目标定位算法（四）之移动目标定位算法

文章目录

移动目标定位算法

1.移动目标计算机仿真建模

2.基于距离观测的运动目标定位

3.纯方位角的运动目标定位

目标定位算法（四）之移动目标定位算法相关推荐

最新文章

热门文章