CN112665464A - 一种履带式单兵智能移动靶 - Google Patents

Abstract

本发明公开了一种履带式单兵智能移动靶，由智能运动平台模块、动力伺服机构、避障模块、自动导航控制系统模块、电气控制系统模块、RTK定位模块、姿态控制、通信链路模块、UWB跟随模块、上位机软件系统模块、手持遥控器模块、靶标模块等组成，是一款具有可模拟假想敌人员目标外形特征、战术动作特性及毁伤状态动作模拟的单兵智能靶标，可满足新的训练理念需求、新的战术射击需求，符合战时心里训练要求的实战化训练智能靶标，单兵智能移动靶(履带式)的研制可满足新编成部队的战术射击训练，对部队的作战训练具有重大意义。

Description

一种履带式单兵智能移动靶

技术领域

本发明涉及一种均是训练设备，尤其涉及一种履带式单兵智能移动靶。

背景技术

为军队确保“能打仗、打胜仗”，我军进行了进一步优化编制结构，注重提高质量，使部队建设的科技含量明显增大，在精兵之路上迈出新的步伐。随着军事改革的完成，形成了新的作战单位及作战体系，也提出了新的训练要求。以往我军基本上行使用固定式起倒靶(单兵起倒靶)进行战术训练射击，降低了射击的难度，脱离实战，难以起到锤炼部队作战技能的效果。根据新的训练需求急需一款具有可模拟假想敌人员野外做横向、进攻、无规则运动的单兵目标战术靶标。

发明内容

本发明的目的就在于为了解决上述问题而提供一种履带式单兵智能移动靶。

本发明通过以下技术方案来实现上述目的：

本发明包括智能运动平台模块、动力伺服机构、电气控制系统、电源模块和靶标模块，所述智能运动平台模块采用履带式结构，并能够实现原地转弯及30°以上爬坡能力，所述动力伺服机构驱动所述智能运动平台模块，所述电气控制系统、所述电源模块和所述靶标安装架搭载于所述智能运动平台模块上，所述电气控制系统由壁障模块、自动导航模块、RTK定位模块、姿态控制模块、通信链路模块、UWB跟随模块、上位机软件系统模块和手持遥控器，所述动力伺服机构具有制动模块；

所述壁障模块采用在单兵智能移动靶前端安装激光测距仪，通过激光测距仪测得的智单兵智能移动靶距离前方障碍物的距离来判断车的行驶状态，当距离低于设定的停车距离时，自动导航控制系统发停车指令；当障碍物移除，单兵智能移动靶车会继续行驶；激光测距仪可识别车轮正前方＞180°的障碍物进行避障识别；

所述自动导航模块包含多传感器陀螺仪，磁力计，加速度计、卫星定位接收机，在自动导航过程中，通过上位机规划一条路径，并将航点经纬度坐标上传到控制器中，控制器通过磁力计或其他获取航向的传感器得到车的目标指向，通过卫星接收机获取实时位置信息，解算出相对于航点的相对坐标，通过PID控制器和导航算法，使单兵智能移动靶可以精准的沿着航线行驶；

所述RTK定位模块包含基站和移动站，基准站先定位，实时处理两个测量站载波相位观测量的差分方法，将基准站采集的载波相位发给移动站，进行求差解算坐标；

所述姿态控制模块通过加速度计、陀螺仪、磁力计输出单兵智能移动靶的俯仰，滚转，偏航姿态信息，对于移动靶车，通过PID控制器来控制车辆目标航向角和当前航向角的偏差值来实现车辆按照规划好的航线来行驶；

所述通信链路模块包括遥控器和单兵智能移动靶车，航线规划上位机和单兵智能移动靶车，起倒靶和上位机；

所述UWB跟随模块包括四个，其中三个设置于智能运动平台模块上，另外一个设置于士兵身上，得到一个绝对的定位，从而解算出移动车辆和士兵之间的航向，以及相对的水平距离信息，通过控制航向和相对位置，实现跟随动作；

所述上位机软件系统模块通过4G/数传电台通信，上位机上可以显示车辆的位置信息，车速，姿态，等实时状态信息；上位机在地图上规划航线，将航点信息发送到智能移动靶车，控制器配合磁罗盘或者其他可以得到航向信息的设备以及卫星接收机从而可以使车辆进行自动导航；

所述手持遥控器进行手动控制，包括前进，后退，转向控制，切换自动导航模式以及在紧急情况下的手动制动；

所述靶标模块通过采集导电靶标被击中的位置，输出两种状态信息，发送到自动导航控制器，从而使车辆做出相应的状态改变；击中头部，智能移动把车处于停止状态，重置后返回到起点，重新按照航线行驶；击中非头部位置，单兵智能移动靶，减速到指定安全区域，重置后，返回到击中位置继续沿航线行驶。

所述履带式单兵智能移动靶的系统包括功能应用层、数据服务层、数据传输层、执行层，所述功能应用层包括运动参数配置、地图构建、路径规划、编队设置、靶标隐显设置控制、状态数据显示、数据存储及记录；所述数据服务层对设备系统所涉及到的数据进行记录与存储管理，提供数据资源的共享服务；所述数据传输层基于军用TD-LTE4G无线通信网络体制，通过核心网和多基站间的自组网，实现训练场区内各类模拟终端演训信息的高效传输，所述执行层为终端接入，实现靶标设备终端及采集器终端的接入。

所述履带式单兵智能移动靶的系统采用自主路径规划和人工势场方法，所述自主路径规划：对于已建模环境，单兵智能移动靶在该环境中运动通过实时路径规划和路径导航的方式实现，对于随机选定的目标点，根据栅格地图可以自动规划生成一条由起点指向终点，且避开障碍的路径，对于存在动态障碍物的环境，根据障碍物的运动参数，估计单兵移动靶到达障碍物附近时障碍物的位置，根据障碍物可能出现的位置规划路径；所述人工势场方法：将靶车在靶场环境中的运动作为在一种虚拟人工受力场中运动；其中，目标点对靶车产生引力，障碍点对靶车产生斥力，靶车在合力的作用下避开障碍物，向目标运动，引力和斥力有定义的势函数计算：U(x)＝U_att(q)+U_rep(q)；

式中U为总的势函数，U_att为引力势函数，U_rep为斥力势函数，q为靶车位置；

上式中，k_att为引力系数，k_rep为斥力系数，q_g为目标点位置，q₀为障碍点位置。ρ₀为斥力势场的作用距离。当靶场与障碍物的距离在此范围以内，则开始考虑斥力影响；

根据靶场环境的特殊性，采用全场均匀的引力势函数，斥力函数不变；对势函数进行微分，则可得到引力和斥力函数为：

F_att＝-k_att·F₀

则靶车受到的合力方向为：

其中n为斥力势函数作用范围内的障碍物数量；

障碍物可通过在栅格地图中手动指定中心点，或者通过软件算法计算得到障碍物的几何中心作为其中心点，其他靶车则被作为动态障碍物，其本身也在周围产生斥力势场；完成靶车的合力方向后，接下来靶车的运动控制，采用基于目标点的导航控制方法，将原来指向目标点的目标航向替换成现在的合力方向。

所述壁障模块在对于已知环境采用激光雷达实时监测动态障碍物并进行有效规避；在未知环境下，激光雷达同时担负探测环境和发现障碍物的任务，运动靶车上使用激光雷达避障碍和防撞，控制系统响应的功能实现。所述自动导航模块采用IMU和里程计数据融合的处理方式推算单兵智能移动靶的航迹；融合方法选用基于加权置信度平均法或Kalman滤波法；根据前一时刻计算出靶车定位坐标和朝向以及当前靶车坐标，可计算出当前靶车的航向

其中，[x_t,y_t],[x_t-1,y_t-1]分别为车体当前时刻和上一时刻的位置坐标，航向范围为

采用IMU惯导模块+里程计的方式进行辅助定位，靶车安装有广电编码检测运动的距离，若编码器采集数据的时间足够短，则靶车的实际运动情况与理论模型十分接近，这样能计算传感器单位采样时间ΔT内靶车的位置、航向角的变化，进行累加，得到任意时刻靶车在世界参考坐标系的位置、航向角。

本发明的有益效果在于：

本发明是一种履带式单兵智能移动靶，与现有技术相比，本发明是一款具有可模拟假想敌人员目标外形特征、战术动作特性及毁伤状态动作模拟的单兵智能靶标，可满足新的训练理念需求、新的战术射击需求，符合战时心里训练要求的实战化训练智能靶标，单兵智能移动靶(履带式)的研制可满足新编成部队的战术射击训练，对部队的作战训练具有重大意义。

附图说明

图1是本发明的侧面结构示意图；图2是本发明的俯视结构示意图；图3是本发明功能应用结构示意图；图4是本发明运行设计流程图；图5是本发明激光雷达探测障碍物区域划分图；图6是本发明障碍物规避示意图；图7是本发明北斗卫星信号穿过电离层示意图；图8是本发明差分技术原理示意图。

具体实施方式

下面结合附图对本发明作进一步说明：

如图1和2所示：包括智能运动平台模块1、动力伺服机构2、电气控制系统3、电源模块4和靶标模块5，所述智能运动平台模块采用履带式结构，并能够实现原地转弯及30°以上爬坡能力，所述动力伺服机构驱动所述智能运动平台模块，所述电气控制系统、所述电源模块和所述靶标安装架搭载于所述智能运动平台模块上，所述电气控制系统由壁障模块、自动导航模块、RTK定位模块、姿态控制模块、通信链路模块、UWB跟随模块、上位机软件系统模块和手持遥控器，所述动力伺服机构具有制动模块；

(1)智能运动平台模块：采用履带式结构方式，车身尺寸：1.15m*0.678m*0.584m，车身采用大量的锻铝铝合金加工，已达到车身轻便，重心低，转向迅速的特点；履带采用一体成型橡胶履带(内嵌凯夫拉纤维)，可实现原地转弯及30°以上爬坡能力。(2)动力伺服机构：采用2台工业电机合并额定功率4w，最大功率8.6kw。设计动力传输路径:电机输出轴-驱动轴-小链轮-链条-大链轮-履带驱动盘-履带，最大速度可达到最高速6.94m/s(人工)，自动驾驶速度也可达5.5m/s。(3)避障模块：采用在单兵智能移动靶前端安装激光测距仪，通过激光测距仪测得的智单兵智能移动靶距离前方障碍物的距离来判断车的行驶状态，当距离低于设定的停车距离时，自动导航控制系统发停车指令；当障碍物移除，单兵智能移动靶车会继续行驶。激光测距仪可识别车轮正前方＞180°的障碍物进行避障识别。(4)电源模块：电源模块采用72V50Ah锂电池组进行供电，可实现连续使用时间＞8h，连续行驶里程＞30km。(5)自动导航模块：设计使用32位STM32F427核心外加FPU(浮点运算单元)和32位STM32F103故障保护协处理器；传感器：MPU6000三轴加速度计/陀螺仪，LSM303D 14位加速度计/磁力计；自动导航控制系统包含多传感器陀螺仪，磁力计，加速度计等，还包括卫星定位接收机。在自动导航过程中，通过上位机规划一条路径，并将航点经纬度坐标上传到控制器中，控制器通过磁力计或其他获取航向的传感器得到车的目标指向，通过卫星接收机获取实时位置信息，解算出相对于航点的相对坐标，通过PID控制器和导航算法，使单兵智能移动靶可以精准的沿着航线行驶。(6)电气控制系统模块：采用arm高速芯片控制，高速力矩输出平稳，转速稳定。(7)RTK定位模块：采用高精度定位接收机，坐标基准：WGS-84；定位精度可以达到静态小于50cm，动态小于20cm；更新速率：5hz；修正后定位精度可达≤5cm。RTK载波相位差分技术，包含基站和移动站，基准站先定位，实时处理两个测量站载波相位观测量的差分方法，将基准站采集的载波相位发给移动站，进行求差解算坐标。(8)姿态控制模块：姿态控制部分，通过加速度计，陀螺仪，磁力计可以输出单兵智能移动靶的俯仰，滚转，偏航姿态信息，对于移动靶车，通过PID控制器来控制车辆目标航向角和当前航向角的偏差值来实现车辆按照规划好的航线来行驶。(9)通信链路模块：单兵智能移动靶的通信链路包括：遥控器和单兵智能移动靶车，航线规划上位机和单兵智能移动靶车，起倒靶和上位机。通信链路解决上位机以及遥控器手动模式与移动靶车之间的数据传输通信的问题。(10)UWB跟随模块：UWB是一种区域绝对定位方案，目前方案是在车上放置三个UWB模块，士兵身上放置一个模块，可以得到一个绝对的定位，从而解算出移动车辆和士兵之间的航向问题，以及相对的水平距离信息，这样可以通过控制航向和相对位置，从而实现跟随动作。(11)制动模块：单兵智能移动靶采用双路液压碟刹进行刹车控制，可实现在靶车高速行驶中进行快速制动刹车控制。(12)上位机软件系统模块：上位机和单兵智能移动靶之间通过4G/数传电台通信，上位机上可以显示车辆的位置信息，车速，姿态，等实时状态信息；上位机也可以在地图上规划航线，将航点信息发送到智能移动靶车，控制器配合磁罗盘或者其他可以得到航向信息的设备以及卫星接收机从而可以使车辆进行自动导航。(13)手持遥控器：单兵智能移动靶可以通过遥控器进行手动控制，包括前进，后退，转向控制，切换自动导航模式以及在紧急情况下的手动制动。(14)靶标模块：靶标通过采集导电靶标被击中的位置，输出两种状态信息，发送到自动导航控制器，从而使车辆做出相应的状态改变。击中头部，智能移动把车处于停止状态，重置后返回到起点，重新按照航线行驶；击中非头部位置，单兵智能移动靶，减速到指定安全区域，重置后，返回到击中位置继续沿航线行驶。

单兵智能移动靶(履带式)按照功能应用层、数据服务层、数据传输层、终端接入层执行四层次的总体技术架构进行设计如图3所示；

1)应用功能层：应用功能层主要面向演训组织和导演调理人员，提供车体参数配置(包括运动参数配置、地图构建、路径规划、编队设置)、靶标隐显设置控制、状态数据显示、数据存储及记录等功能；2)数据服务层：数据服务层是对设备系统所涉及到的数据进行记录与存储管理，提供数据资源的共享服务。3)数据传输层：数据传输层主要基于军用TD-LTE4G无线通信网络体制，通过核心网和多基站间的自组网，实现训练场区内各类模拟终端演训信息的高效传输。4)执行层：终端接入层主要实现靶标设备终端及采集器终端的接入。

单兵智能移动靶设计组成包括：计算机及软件分系统、手持遥控器、智能运动平台车、锂电池组、射击靶标、电气控制分系统、定位基站、天线、防弹组件等组成。单兵智能移动靶设计运行如图4所示；

自主路径规划：对于已建模环境，单兵智能移动靶再改环境中运动可通过实时路径规划和路径导航的技术方式来实现。对于随机选定的目标点，根据栅格地图可以自动规划生成一条由起点指向终点，且避开障碍的路径。现有基于栅格的路径算法有很多，对于已知的静态环境，A*算法能够最有效的求解出两点之间的最短路径，对于存在动态障碍物的环境，可以根据障碍物的运动参数，估计单兵移动靶到达障碍物附近时障碍物的位置，构建“时空栅格地图”，根据障碍物可能出现的位置规划路径。A*算法的整体框架是一个图的遍历搜索算法，不同于大多数图的搜索算法，采用了启发函数来估计地图上的任意点离目标点的远近程度。通过这种启发式，可以协调选择最好的方向搜索。如果过程中失败，还可以选其他路径搜索，直到找到最优的路径。A*算法在人工智能中是一种典型的启发式搜索算法，其中启发中的估价用估价函数表示：f(x)＝g(x)+h(x)；

其中，f(x)是从初始点经由节点X到目标点的估价函数，g(x)是在状态空间中从初始点到节点X的实际代价，h(x)是从节点X到目标点最优路径的估计代价。当估计代价不大于节点X到目标点的实际距离值时，搜索的点数多，效率低，但是能得到最优解；当h(x)大于节点X到目标点的实际距离值时，搜索点数少，效率高，但是不能保证得到最优解；对于实际环境中可以选取两节点间的欧几里得距离(直线距离)作为估计代价，即

其中，(x_X，y_X)是节点X的坐标，(x_G,y_G)是目标点G的坐标。

人工势场方法

靶车在靶场环境简单、障碍物较少的情况下，采用人工势场法。人工势场法是将靶车在靶场环境中的运动作为在一种虚拟人工受力场中运动。其中。目标点对靶车产生引力，障碍点对靶车产生斥力，靶车在合力的作用下避开障碍物，向目标运动。引力和斥力有定义的势函数计算：U(x)＝U_att(q)+U_rep(q)；

式中U为总的势函数，U_att为引力势函数，U_rep为斥力势函数，q为靶车位置。

上式中，k_att为引力系数，k_rep为斥力系数，q_g为目标点位置，q₀为障碍点位置。ρ₀为斥力势场的作用距离。当靶场与障碍物的距离在此范围以内，则开始考虑斥力影响。根据靶场环境的特殊性，采用全场均匀的引力势函数，斥力函数不变。对势函数进行微分，则可得到引力和斥力函数为：

F_att＝-k_att·F₀

则靶车受到的合力方向为：

其中n为斥力势函数作用范围内的障碍物数量。

障碍物可通过在栅格地图中手动指定中心点，或者通过软件算法计算得到障碍物的几何中心作为其中心点。其他靶车则被作为动态障碍物(其实时位置信息通过系统广播获得)，其本身也在周围产生斥力势场。完成靶车的合力方向后，接下来靶车的运动控制，采用基于目标点的导航控制方法，将原来指向目标点的目标航向替换成现在的合力方向。

动态避障技术及算法实现途径：

对于已知环境，采用启发式路径规划算法、手动路径规划等。这种情况下，需采用激光雷达实时监测动态障碍物并进行有效规避；在未知环境下，激光雷达同时担负探测环境和发现障碍物的任务。运动靶车上使用激光雷达避障碍和防撞，控制系统响应的功能实现。

对激光雷达的前方探测划定区域，分为D(dangerous)、W(warning)、U(uncertain)以及S(safe)等四种区域，如图5所示。对于出现在D区域和W区域的障碍物，车体根据安全控制规划执行相应减速或者在确保U区域和S区域无障碍的情况下转弯规避。控制方式为：

其中[X_obj，Y_obj]为进入W或D区域的障碍物坐标，D_o为危险距离阈值，D_w为报警距离阈值。设置D_o≥1.2m，可保证靶车与障碍物的距离不小于1m。

对于进入U区域的障碍物，需要根据规划路径与该区域的关系决定车体控制行为。如果规划路径经过该障碍物，则可采取延迟转弯或者在保证运动安全的情况下，加大转弯控制量。对于处于S区域的物体，可以忽略。

靶车在规避障碍物过程中的转弯控制，基于激光雷达获得的障碍物信息做出。靶车通过寻找就近的子目标点并运动到该位置，然后基于新位置和障碍物信息设置新的子目标点来分阶段完成障碍物规避。控制系统以车体朝向中轴线为起点，分别计算两侧障碍物测量的相邻点距离，直到找到某两个相邻点距离大于某一阈值大于两倍车体宽度，该地点距离中轴线较劲的一侧为车体运动方向，如图6所示；

激光雷达的使用受到车体俯仰、路面起伏的影响，具有一定的局限性。使用过程中应配合IMU模块测量的俯仰角度。根据结构设计方案，激光雷达安装位置高度为离地0.3m。因此对于低于0.3m的障碍物，激光雷达扫描平面无法覆盖。该应用的局限性对靶场地面有一定要求。但是实际上大多数靶场是能够满足这一使用条件的。通过设置激光雷达探测障碍信息的距离阈值，可使得该算法在不同的靶场环境(主要体现在障碍物分布间距不同)中很好地使用。

3.自主导航运动控制技术及实现途径

导航控制首先要解决的是定位问题。采用动态定位标签的姿态测量方法相比静态姿态测量方法节省定位成本，但需要在运动中对相邻时刻的定位数据进行计算得出当前朝向和靶车中心点位置坐标。当定位信号不佳时，可以采用IMU和里程计数据融合的处理方式推算单兵智能移动靶的航迹。融合方法可选用基于加权置信度平均法或Kalman滤波法。

根据前一时刻计算出靶车定位坐标和朝向以及当前靶车坐标，可计算出当前靶车的航向

其中，[x_t,y_t],[x_t-1,y_t-1]分别为车体当前时刻和上一时刻的位置坐标，航向范围为

采用IMU惯导模块+里程计的方式进行辅助定位。靶车安装有广电编码检测运动的距离，若编码器采集数据的时间足够短，则靶车的实际运动情况与理论模型是十分接近的，这样可以计算传感器单位采样时间ΔT内靶车的位置、航向角的变化，进行累加，得到任意时刻靶车在世界参考坐标系的位置、航向角。基于IMU+里程计的航迹推算方法虽然存在积累误差且不可校正，但在短时间内作为辅助定位是可行的。

辅助定位采用IMU+里程计数据，通过建立差速转向车体的运动学模型来计算获得车体的姿态信息。运动学模型为：

其中，ΔL，ΔR分别表示左右轮的里程计数，k为比例系数，D为轮轴距。

单兵智能移动靶的运动学模型一般有种：直线模型和弧线模型。直线模型具有运算相对简单、估计精度低的特点；弧线模型具有运算相对复杂、估计精度高的特点。

定义全局地面笛卡尔坐标系为I：O-XYZ,其中Z轴方向垂直向上，在车体质心建立附体直角坐标系B：G-xyz，定义

表示姿态角，其中φ，θ，

分别为滚转角Roll、俯仰角Pitch和方位角Yaw。从B到I的坐标系变换的旋转矩阵为：

用L和W分别表示径向和横向的轮距，车体中心速度用VG表示，给定位车体本体坐标系B中的车体中心速度VG和角速度

沿x轴方向车体左右侧车轮速度可表示为：

转动车体在转弯过程中必然出现车轮滑动。这一过程通常采用车轮的滑移率来描述，这里将车轮的滑移率定义为实际车速与车轮转速的百分比：

其中ω_i为车轮角速度，r为车轮的有效半径，ν_ix为车轮的实际径向速度。可以设定，左侧车轮的滑移率λ_L＝λ₁,右侧车轮的滑移率λ_R＝λ₂。当车轮处于牵引状态，车轮的滑移率λ∈[0,1)；当车轮处于制动状态，滑移率λ∈(-∞，0]。为方便运算可将滑移率约束在一定范围内，即当制动时λ＜-1，则取λ＝-1，因此滑移率的方位为λ∈[-1,1]。

将车体的瞬时旋转中心(Instantaneous Center of Rotation,ICR)表示为ICR_G,将左侧车轮与地面接触点和右侧车轮与店面接触点所在的瞬心分别表示为ICR_l和ICR_r，当ICR_G，ICR_l和ICR_r同处于平行于y轴的一条直线上，将ICR_G，ICR_l和ICR_r在B坐标系中的坐标分别表示为：(x_lc,y_lc),(x_rc,y_rc)和(x_Gc,y_Gc)。滑动转向车体的动力学分析。根据ICR位置，车轮与地面之间的径向滑移速度可表示为：

根据上式，当

时，则有：

令F_ix和F_iy分别为车体第i个车轮与地面接触点径向和横向的作用力，将第i个车轮于地面间的径向滑动摩擦力表示为F_ix＝N_iμ_i，其中N_i为车轮正压力，μ_i为滑动摩擦系数。当车体行驶于坚硬路面时，一般认为滑动摩擦系数μ_i是车轮滑动率λ的函数。

将μ-λ曲线线性近似为：

其中K为车轮的刚度系数，λ_m是由最大摩擦系数得出的滑移率，μ_s为滑动摩擦系数，当滑移率λ≤λ_m时，车轮运行在非平稳区域。假设滑动摩擦的摩擦系数μ_s是峰值摩擦系数μ_p的某一比例，即μ_s＝αμ_p＝αKλ_m，其中0≤α≤1。则为：μ(λ)＝K[σ₁(λ)+σ₂(λ)sgn(λ)λ]

径向滑动摩擦力F_ix和横向滑动摩擦力F_iy是相关的，其数值等于总滑动摩擦力在各个方向的分量大小，即F_ix＝F_ircosβ_i,F_iy＝F_irsinβ_i，其中F_ir是滑动摩擦力的合理，β_i是车轮的打滑角度，是车轮与地面接触点与瞬时旋转中心的连线以及车轮转动轴的中心线之间的夹角。

导航控制算法中对靶车运动过程中两个偏差进行控制，一个是航向偏差量

一个是距离偏离量。航向偏量为目标航向

与车体航向

之间的偏差表示为：

采用正切函数作为非线性控制器，由于正切函数具有周期性的单调上升性质，将偏量进行缩放后代入正切函数，能够与实际控制量具有一致性。

其中k_la为比例控制系数，A₀为控制量安全阈值。

其中V_t是由速度PID控制产生的实时车速控制输出量，CtrlL，CtrlR分为左右轮的控制输出量。可得，当车体航向与目标航向夹角＜180°时，车体控制量表现为右拐；当＞180°时，车体控制量表现为左拐。实际上这种控制使得车体航向以最快的方式朝向目标航向。即为基于目标点的导航控制。

航向偏量和距离偏量在目标直线导航控制和目标圆弧导航控制中分别对应不同的物理量。在目标直线导航控制中，航向偏量和距离偏量分别为：

其中，[x_D,y_D],[x_S,y_S]分别表示直线目标端点和起始端点的坐标；[x_t,y_t]为车体当前位置坐标。

航向偏量的控制律可使用正切函数来计算，而距离偏量则使用反正切函数来实现，反正切函数具有控制上下限阈值，对微小量反应灵敏的特点。

其中，k_la，k_ld分别为航向偏量和距离偏量的比例控制系数。

最终的控制输出为两种偏量控制律的叠加。

在目标圆弧导航控制中，目标航向分为顺时针方向和逆时针方向两种状态为：

式中，[x_c,y_c]为圆弧的圆心坐标。因此航向偏量和距离偏量分别为：

Ad＝k_ad·(Dis([x_t,y_t],[x_c,y_c])-Dis([x_D,y_D],[x_c,y_c])+Δd)

k_aa，k_ad分别表示航向，距离偏量的比例控制系数。[x_D,y_D]为圆弧终点坐标，Dis函数为两坐标点的欧式距离，Δd为校正偏移量。

最终的控制输出为两种偏量控制律的叠加。

通过目标点、目标直线和目标圆弧的组合，可构造出任意路径。因此整体路径的导航控制即可分解为各子段路径的导航控制来按时按序完成。只需在各段加入导航切换机制即可。

高精度定位技术及实现途径

北斗定位技术原理：

在平面上，两条非平行的直线可以确定一个点，而在在空间中，两个相交的曲面可以确定一条曲线，三个相交的曲面就可以确定两个点，北斗导航定位系统就是根据这个原理来进行定位解算的。假设已知三个卫星的空间位置，以及北斗定位接收机到这三个卫星的距离，那么，北斗接收机肯定就位于以三个卫星为球心，以卫星到接收机距离为半径的三个球的交点上，可以根据先验知识很容易的排除另外一个点，因为，那个点并不会在地球上。通常情况下，需要四颗卫星才可以进行定位解算，因为，要将每颗卫星的时延也作为一个未知变量参与方程组的求解。因此，要求得北斗定位接收机的具体位置信息，首先需要计算出两部分的数据：参与定位解算的北斗卫星在空间中的具体位置；参与定位解算的卫星与我们的北斗定位接收机之间的距离；

伪距是指在卫星定位过程中，地面接收机到卫星之间的大概距离。伪距可以通过指定时刻卫星同用户间的相对时差与信号自卫星到用户间的传播时延之和再乘以光速来求得，其表达式为：

ρ＝r+δt_u-δtⁿ+I+T+ε_p (1-1)

式中，r表示接收机到卫星之间的几何距离；t_u和tⁿ分别表示接收机的钟差和卫星的钟差所造成的伪距变化量，上标n是用来区分不同的卫星的；I和T分别表示由电离层和对流层所造成的大气传播延迟叠加在伪距上产生的伪距变化量；ε_p表示所有未直接体现在式子(1-1)中的误差的总和。假设一台接收机在某一观测时刻可对N颗可见卫星进行伪距测量，那么我们将式子(1-1)改写成如下形式：

式中，n＝1,2,3...N是对不同可见卫星的编号。那么经过对流层、电离层和卫星钟差校正后的伪距测量值为

为：

校正后的伪距观测方程式为：

rⁿ就是接收机到卫星之间的几何距离。

导航卫星的开普勒模型，可以通过开普勒模型来计算出导航卫星的准确位置。

(1)计算规化时间：t_k＝t-t_oe (1-5)

(2)计算参与定位解算的北斗卫星的平均角速度：n＝n₀+Δn (1-6)

其中，n₀就是该卫星的平均角速度，而Δn可以由星历直接获得，n₀的计算

公式为：

其中n₀为地心引力常数，a_s可以由星历直接获得。

(3)计算卫星信号发射时刻的平近点角：

将星历给出的M₀带入以下线性模型:M_k＝M₀+nt_k (1-8)

(4)计算卫星信号发射时刻的偏近点角：E_k＝M_k+esinE_k-1 (1-9)

E₀＝M_k经过2～3次迭代后，便可以得到相当精确的解。

(5)计算卫星信号发射时刻的真近点角：

(6)计算卫星信号发射时刻的纬度幅角参数：Φ_k＝v_k+ω (1-11)

(7)计算卫星信号发射时刻改正项：

δμ_k＝C_ussin(2Φ_κ)+C_uscos(2Φ_κ) (1-12)

δr_k＝C_rssin(2Φ_κ)+C_rscos(2Φ_κ) (1-13)

δi_k＝C_issin(2Φ_κ)+C_iscos(2Φ_κ) (1-14)

其中：δμ_k为纬度幅角改正项；

δr_k为径向改正项；δi_k为轨道倾角改正项。

(8)计算改正后的纬度幅角、径向和轨道倾角：

μ_k＝Φ_κ+δμ_κ (1-15)

r_k＝a_s(1-e_scosE_k)+δ_rk (1-16)

(9)计算卫星轨道面内的坐标：

x_k＝r_kcosμ_κ

y_k＝r_ksinμ_κ (1-18)

(10)计算修正后的升交点经度：

(11)计算卫星在地心地固坐标系中的坐标：

X_k＝x_kcosΩ_k-y_kcosi_ksinΩ_k

Y_k＝x_ksinΩ_k-y_kcosi_kcosΩ_k

Z_k＝y_ksini_k (1-20)

到此已经可以得出了卫星的准确位置。

北斗接收机定位误差分析：在导航定位系统中，由于测量误差而造成的距离误差，都可以等价为由于伪距测量而带来的误差，对于这些误差我们统称为用户等效距离误差。

(1)电离层误差：电离层，地球大气区域的一种。电离层指的是地球表面50公里左右开始至地球表面1000公里左右的大气区域。在太阳辐射中紫外线和X射线的作用下，导致电离层中的部分气体分子产生电离化，产生了一些自由电子。由于电离层中大量自由电子的存在，对北斗卫星信号的传播速度造成了一定的影响，进而造成了北斗定位的电离层误差。目前，修正电离层误差的方法主要有三种：

(2)利用双频接收机修正电离层误差，这是因为，卫星信号在电离层中的散射程度是跟卫星信号的频率有关的，可以根据不同频率的卫星信号的延迟量的不同，修正电离层对定位精度的影响；

(3)模型法，稍后我们会介绍一种电离层修正模型——Klobuchar模型；

(4)差分定位原理，在短距离内，电离层的延时有一定的规律可循，所以可以利用差分站来进行差分定位，进而消除电离层误差。现在对第二个方法，即使用模型法消除电离层误差作简要介绍，这里选择的修正模型是Klobuchar电离层时延修正模型。该修正模型是利用电离层一天中时延的变化规律来去除电离层误差的，因为在夜间，电离层的时延是比较稳定的，保持在5ns左右；而在白天，电离层的时延最大值，出现在当地的下午两点；根据这两点，可以将所有时间电离层延时的变化规律用式子(1-2-1)表示：

其中，A表示电离层延时函数振幅，单位：s；

P表示电离层延时函数周期，单位s；t’表示测量时刻t时电离层穿刺点的时刻，单位s。A和P可以通过以下的方法来计算：

其中，a_i和β_i是导航电文中提供的数据；φ_m是电离层穿刺点的地磁纬度。式子(1-2-1)是任意一时刻t时天顶方向的电离层延时改正。而任意一时刻t时仰角为E的观测方向电离层延时改正为：

T′_g＝SF·T_g (1-2-4)

其中，

E为观测卫星方向的仰角。

由上述公式计算T′_g时，为了提高计算的速度，我们将T_g的表达式简化为：

其中，

通过上述介绍，我们发现要想计算出信号穿过电离层的延时，只需计算出t′和φ_m的值。

如图7所示，k是北斗定位接收机位置，k′是北斗信号穿过电离层的位置，两者之间的角度可以这样来求得：

其中，E是观测卫星的仰角，那么k′点的地心纬度和经度分别为：

其中，

是北斗定位接收机k点的纬度，λ_k是北斗定位接收机k点的经度，而δ是观测卫星方向的方位角。那么北斗卫星信号经过电离层的位置k′点的地方时就是：

其中，UT为观测时刻t时的世界时。

以利用地磁北极的经纬度(纬度78.4°N，经度291.0°E)来计算北斗信号经过电离层的位置k′点的地磁纬度：

在求出t′和

之后，就可以求得信号穿过电离层的延时T′_g。

(5)对流层误差：对流层处在大气层的最底层。对流层中大气的状态十分复杂，虽然其下边界紧靠地面，但是上边界却没有一个固定的值，对流层的上边界不但跟纬度高低有着密切联系，而且跟季节也有着一定的关系；在对流层中，气温一般随着高度的降低而升高，但是，有时候对流层中的气温会出现随着高度的降低而降低这一现象，我们称这种现象为“逆温现象”。在北斗卫星信号穿过对流层的途中，卫星信号的传播速度会发生变化，与此同时，卫星信号的传播方向也会发生变化。种种的不确定性都使得对对流层的误差进行处理存在各种困难。对流层误差也是造成北斗定位误差的主要因素之一。针对对流层对北斗卫星信号的影响，霍普菲尔得(Hopfield)修正模型可以取得一定的修正效果，Hopfield模型的修正方法如下所示：

其中，S是对流层延迟校正值；

E是卫星在站心坐标系中的仰角，单位度；K_d是卫星位于天顶方向时的对流层干延迟改正；K_ω是卫星位于天顶方向时的对流层湿延迟改正。K_d和K_ω的计算公式为：

其中，T_s为测站的热力学温度，单位K；p_s为测站的大气压，单位100Pa；e_s为测站的水气压，单位100Pa；h_s为测站的高程，单位m；h_d为干温度的等效高度，h_d＝40136+148.72(T_s-273.16)(m)；h_ω为水气层的外缘高度，霍普菲尔德建议采用11000m，h_ω＝11000(m)。干温t_s(T_s＝t_s+273.16°)和气压p_s见可利用温度计和气压计在北斗接收机附近直接测得。水汽压e_s是利用测定站上的干温T_s干湿温度计测得)和湿温T_ω干湿温度计测得)，然后按下列公式得来的，单位为100Pa：

e_s＝w_ω-4.5×10^-4(1+1.68×10^-8T_ω)(T_s-T_ωp_s) (1-2-13)

式中；e_ω为饱和水汽压；

g(T_ω)＝g₁(T_ω)+g₂(T_ω)+g₃(T_ω) (1-2-15)

在实际操作中，干温T_s和湿温T_ω的值可以使用干湿温度计直接测量得到，而高程h_s的值也可以使用高程仪来获得。将这些测得的变量代入式子(1-2-12)-(1-2-18)中进行计算，便可以得到对流层的时间延迟ΔS，我们在对北斗卫星接收机中的伪距进行计算时，将这个延时带来的误差直接删除就好了，进而使得对流层延迟误差得到校正。一般的，在利用Hopfield模型进行了对流层延时校正后，对流层对定位精度造成的的影响基本就修正好了，然而在实际操作中，还存在一些误差会对定位精度造成一定的影响，根据经验可以把精度校正到0.5m之内。

(6)多径效应修正：

北斗信号的多径效应，顾名思义，就是北斗信号在传输时，会有多条时延不同的传输路径，而导致最后北斗接收机接收到的北斗信号是由各路径的信号叠加而成的。在信号传输时，导致有多条路径进行传输的原因有以下几个方面：a.北斗卫星发射北斗信号时，北斗卫星对信号的反射造成的；

b.在北斗信号传输过程中，由于电离层的和对流层的不均匀造成的；

c.在接收时，由于北斗接收机周围的一些建筑群等障碍物的反射而造成的。多径效应的影响跟北斗接收机具体的使用环境有着很大的关系，因此，并没有通用的模型来解决这个问题。目前，解决多径效应造成的误差有下面几个途径：

d.在北斗接收机的天线中添加自动调整天线方向的算法，对天线的接收方向进行调整，抑制干扰信号的接收，完成对有用信号的抗干扰的目的；

e.使用相位测距的方式对伪距进行测量。

(7)卫星星历误差：

由于北斗卫星所播报的卫星星历都是预测的星历，所以并不能保证北斗卫星星历的完全可靠性，因为，卫星在空间中运行时，受到的外力情况十分的复杂。预测的北斗卫星星历存在的误差，就是星历误差，也称为轨道误差。北斗定位导航系统的工作原理是把北斗卫星的当前位置作为已知的基准值，通过计算求得北斗接收机的实际位置，这种情况下，我们接受到的卫星星历存在误差，那么我们计算卫星的位置信息时就会产生一定的误差，通过卫星位置计算得到的北斗接收机位置就不是那么精确了。根据观测方程式，如果卫星的坐标(x^j,y^j,z^j)的误差为δx^j,δy^j,δz^j，引起的距离误差是

我们假设卫星的坐标的均方差为σ_x,σ_y,σ_z，那么它的距离的均方差为：

我们假设σ_xj≈σ_yj≈σ_zj≈σ_j；那么由于

所以

通过上面的推导，我们可以得到以下的结论，由于北斗卫星坐标存在误差，造成的伪距测量的误差可以用卫星三个方向误差的均值来衡量。也就是说，假设北斗卫星坐标的平均误差为10～20m，那么造成的定位误差也就是10～20m。星历误差其实就是卫星轨道上的误差，这是一种系统级的误差，对定位精度的不良影响很大。但是我们要处理卫星运行轨道上的误差是十分困难的，因为，卫星在空间中运行时，会受到各个星体和其他卫星的外力干扰，完全准确的测量这些外力的难度很大，也很难设计出一个模型来计算这些外力。目前，卫星星历造成的定位误差大概在15～45米左右，但是随着定轨技术研究的不断深入，卫星星历造成的定位误差已经可以缩小到5～10米左右。

(8)卫星钟差：每颗北斗卫星上都有一个时间计时装置，掌控着卫星上跟时间有关的所有信息，也就是卫星钟，虽然卫星钟的工作性能十分稳定，但是考虑到北斗卫星传播速度这一巨大的相乘参数，即使1us的时钟误差都会造成300米的伪距测量误差。不过好在卫星钟差有着固定的修正模型，地面控制站会将时钟的校正参数发送给卫星，卫星再将校正参数转播出来。这些校正参数由北斗接收机用二阶多项式来实现：

Δt_s＝a₀+a₁(t-t_OC)+a₂(t_OC)² (1-2-20)

其中，a₀为时间偏差，也就是所谓的钟差；a₁为a₀的变化率，也就是所谓的钟速；a₂为a₁的变化率，也就是所谓的钟速变化率。

(9)接收机钟差：

卫星有卫星的钟差，接收机也有自身的钟差，因为，接收机的时间系统和卫星的时间系统是没有联系的，两个时间系统之间的差值就是我们所说的接收机钟差。类似的，10us的接收机钟差也是会造成300米的伪距测量误差，影响还是比较严重的。但是，对于一台北斗定位接收机来说，它和所有卫星的钟差参数应该是一样的，所以，我们不必特意的针对这一问题设计特定的模型来修正，只需将它放入定位解算时伪距方程组中的时延变量中去，连同北斗接收机的三维位置一起解算出来就可以了。

(10)接收机观测误差：

北斗卫星接收机的观测误差主要是由北斗接收机的天线的相位中心偏移的问题造成的，理论上，天线的几何中心和相位中心是重合的，保持两个中心的重合，就不会存在接收机的观测误差了，但是，天线的相位中心一直在随着信号的变化而实时变化，进而导致了北斗接收机的观测误差，好在这种误差的的影响不是很大。

误差校正：由于误差的存在，卫星导航系统在定位过程中存在较大的定位误差，单点定位的精度不高，无法满足高精度应用场景的需求。针对此类应用，通常采用差分技术来提高定位精度，其技术原理如图8所示；

差分技术利用两台接收机(一般称为基准站接收机与移动站接收机)同时观测卫星信号，利用误差的空间相关性(即在一定基线距离条件下，两台接收机所观测的同一颗卫星其误差基本相同)进行差分计算，有效地消除或降低两站接收机间的公共误差部分，包括星钟误差、星历误差、电离层误差和对流层误差，从而提高了接收机定位精度。

根据予以差分校正的目标参量不同，差分可主要分为位置差分、伪距差分、载波相位差分三种。本发明采用伪距差分

(1)伪距差分技术基本原理：基准站接收机通过卫星信号解码得到伪距测量值；利用基准站的已知坐标及卫星星历信息，可计算出基准站到卫星的真实几何距离，求出该距离与伪距测量值的差值即伪距测量误差；基准站利用数据链将此差值发送给移动站接收机，移动站接收机利用此差值修正其伪距，再进行定位解算，获得其位置。

(2)应用特点：目前应用最广的差分技术，几乎所有的商用差分接收机均采用此技术，定位精度可以达到10cm；随着移动站和基准站之间距离的增加，定位精度下降。

载波相位差分(RTK)技术

(1)载波相位差分(RTK)技术：由于码相位的测量精度在1m左右，载波相位测量值的精度可达几个毫米，因而基于载波相位差分系统通常具有最高的定位精度，可用来实现精密定位。

(2)载波相位差分(RTK)技术基本原理：基准站通过数据链将载波相位测量值实时发送给移动站接收机；移动站接收机将自身的载波相位测量值与所接收的载波相位测量值经差分运算组成差分测量值；求解出初始整周模糊度以及基线向量，以完成高精度定位。应用特点：载波相位差分的定位精度可达到厘米级甚至毫米级；由于误差空间相关性的限制，基准站与移动站在短基线范围内效果较好。

以上显示和描述了本发明的基本原理和主要特征及本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (5)

1.一种履带式单兵智能移动靶，其特征在于：包括智能运动平台模块(1)、动力伺服机构(2)、电气控制系统(3)、电源模块(4)和靶标模块(5)，所述智能运动平台模块采用履带式结构，并能够实现原地转弯及30°以上爬坡能力，所述动力伺服机构驱动所述智能运动平台模块，所述电气控制系统、所述电源模块和所述靶标安装架搭载于所述智能运动平台模块上，所述电气控制系统由壁障模块、自动导航模块、RTK定位模块、姿态控制模块、通信链路模块、UWB跟随模块、上位机软件系统模块和手持遥控器，所述动力伺服机构具有制动模块；

所述壁障模块采用在单兵智能移动靶前端安装激光测距仪，通过激光测距仪测得的智单兵智能移动靶距离前方障碍物的距离来判断车的行驶状态，当距离低于设定的停车距离时，自动导航控制系统发停车指令；当障碍物移除，单兵智能移动靶车会继续行驶；激光测距仪可识别车轮正前方＞180°的障碍物进行避障识别；

所述自动导航模块包含多传感器陀螺仪，磁力计，加速度计、卫星定位接收机，在自动导航过程中，通过上位机规划一条路径，并将航点经纬度坐标上传到控制器中，控制器通过磁力计或其他获取航向的传感器得到车的目标指向，通过卫星接收机获取实时位置信息，解算出相对于航点的相对坐标，通过PID控制器和导航算法，使单兵智能移动靶可以精准的沿着航线行驶；

所述RTK定位模块包含基站和移动站，基准站先定位，实时处理两个测量站载波相位观测量的差分方法，将基准站采集的载波相位发给移动站，进行求差解算坐标；

所述姿态控制模块通过加速度计、陀螺仪、磁力计输出单兵智能移动靶的俯仰，滚转，偏航姿态信息，对于移动靶车，通过PID控制器来控制车辆目标航向角和当前航向角的偏差值来实现车辆按照规划好的航线来行驶；

所述通信链路模块包括遥控器和单兵智能移动靶车，航线规划上位机和单兵智能移动靶车，起倒靶和上位机；

所述UWB跟随模块包括四个，其中三个设置于智能运动平台模块上，另外一个设置于士兵身上，得到一个绝对的定位，从而解算出移动车辆和士兵之间的航向，以及相对的水平距离信息，通过控制航向和相对位置，实现跟随动作；

所述上位机软件系统模块通过4G/数传电台通信，上位机上可以显示车辆的位置信息，车速，姿态，等实时状态信息；上位机在地图上规划航线，将航点信息发送到智能移动靶车，控制器配合磁罗盘或者其他可以得到航向信息的设备以及卫星接收机从而可以使车辆进行自动导航；

所述手持遥控器进行手动控制，包括前进，后退，转向控制，切换自动导航模式以及在紧急情况下的手动制动；

所述靶标模块通过采集导电靶标被击中的位置，输出两种状态信息，发送到自动导航控制器，从而使车辆做出相应的状态改变；击中头部，智能移动把车处于停止状态，重置后返回到起点，重新按照航线行驶；击中非头部位置，单兵智能移动靶，减速到指定安全区域，重置后，返回到击中位置继续沿航线行驶。

2.根据权利要求1所述的履带式单兵智能移动靶，其特征在于：所述履带式单兵智能移动靶的系统包括功能应用层、数据服务层、数据传输层、执行层，所述功能应用层包括运动参数配置、地图构建、路径规划、编队设置、靶标隐显设置控制、状态数据显示、数据存储及记录；所述数据服务层对设备系统所涉及到的数据进行记录与存储管理，提供数据资源的共享服务；所述数据传输层基于军用TD-LTE4G无线通信网络体制，通过核心网和多基站间的自组网，实现训练场区内各类模拟终端演训信息的高效传输，所述执行层为终端接入，实现靶标设备终端及采集器终端的接入。

3.根据权利要求2所述的履带式单兵智能移动靶，其特征在于：所述履带式单兵智能移动靶的系统采用自主路径规划和人工势场方法，所述自主路径规划：对于已建模环境，单兵智能移动靶在该环境中运动通过实时路径规划和路径导航的方式实现，对于随机选定的目标点，根据栅格地图可以自动规划生成一条由起点指向终点，且避开障碍的路径，对于存在动态障碍物的环境，根据障碍物的运动参数，估计单兵移动靶到达障碍物附近时障碍物的位置，根据障碍物可能出现的位置规划路径；所述人工势场方法：将靶车在靶场环境中的运动作为在一种虚拟人工受力场中运动；其中，目标点对靶车产生引力，障碍点对靶车产生斥力，靶车在合力的作用下避开障碍物，向目标运动，引力和斥力有定义的势函数计算：

U(x)＝U_att(q)+U_rep(q)；

式中U为总的势函数，U_att为引力势函数，U_rep为斥力势函数，q为靶车位置；

上式中，k_att为引力系数，k_rep为斥力系数，q_g为目标点位置，q₀为障碍点位置，ρ₀为斥力势场的作用距离，当靶场与障碍物的距离在此范围以内，则开始考虑斥力影响；

根据靶场环境的特殊性，采用全场均匀的引力势函数，斥力函数不变；对势函数进行微分，则可得到引力和斥力函数为：

F_att＝-k_att·F₀

则靶车受到的合力方向为：

其中n为斥力势函数作用范围内的障碍物数量；

障碍物可通过在栅格地图中手动指定中心点，或者通过软件算法计算得到障碍物的几何中心作为其中心点，其他靶车则被作为动态障碍物，其本身也在周围产生斥力势场；

完成靶车的合力方向后，接下来靶车的运动控制，采用基于目标点的导航控制方法，将原来指向目标点的目标航向替换成现在的合力方向。

4.根据权利要求1所述的履带式单兵智能移动靶，其特征在于：所述壁障模块在对于已知环境采用激光雷达实时监测动态障碍物并进行有效规避；在未知环境下，激光雷达同时担负探测环境和发现障碍物的任务，运动靶车上使用激光雷达避障碍和防撞，控制系统响应的功能实现。

5.根据权利要求1所述的履带式单兵智能移动靶，其特征在于：所述自动导航模块采用IMU和里程计数据融合的处理方式推算单兵智能移动靶的航迹；融合方法选用基于加权置信度平均法或Kalman滤波法；根据前一时刻计算出靶车定位坐标和朝向以及当前靶车坐标，可计算出当前靶车的航向

其中，[x_t,y_t],[x_t-1,y_t-1]分别为车体当前时刻和上一时刻的位置坐标，航向范围为

采用IMU惯导模块+里程计的方式进行辅助定位，靶车安装有广电编码检测运动的距离，若编码器采集数据的时间足够短，则靶车的实际运动情况与理论模型十分接近，这样能计算传感器单位采样时间ΔT内靶车的位置、航向角的变化，进行累加，得到任意时刻靶车在世界参考坐标系的位置、航向角。

Images