面向特种车辆应用的三维地形补偿方法和系统与流程

本技术涉及特种车辆应用，尤其涉及面向特种车辆应用的三维地形补偿方法和系统。

背景技术：

1、特种车辆广泛应用于工程施工、农业作业、消防安全以及国防军事等领域，其工作环境往往比较复杂，对于工作效率要求较高，有着自动化和智能化需求。由于特种车辆工作的地形经常起伏不平或者存在较大坡度，对于其自动化控制系统来说存在相当大的挑战，传统的自动化控制系统在复杂地形下容易出现不稳定或者失效的情况，导致较大的作业偏差甚至对行驶安全造成威胁。

技术实现思路

1、本技术旨在至少在一定程度上解决相关技术中的技术问题之一。

2、为此，本技术的第一个目的在于提出一种面向特种车辆应用的三维地形补偿系统，解决了现有方法在复杂地形下容易出现不稳定或者失效的情况，导致较大的作业偏差甚至对行驶安全造成威胁的技术问题，满足了特种车辆在复杂地形下自动化控制需求。

3、本技术的第二个目的在于提出一种面向特种车辆应用的三维地形补偿系统。

4、为达上述目的，本技术第一方面实施例提出了一种面向特种车辆应用的三维地形补偿系统，包括：车身姿态传感器，用于检测特种车辆的车身姿态；gnss双天线，用于接收特种车辆的卫星定位信息；转向控制电机，用于控制特种车辆转向；触控显示屏，用于显示特种车辆的状态信息；核心控制器，用于根据特种车辆的车身姿态、卫星定位信息控制特种车辆运动，通过转向控制电机控制特种车辆转向，并将特种车辆的状态信息显示在触控显示屏上。

5、本技术实施例的面向特种车辆应用的三维地形补偿系统，通过车身姿态传感器与gnss双天线融合进行车辆位姿估计，并对三维地形进行建模与预测，根据地形对控制量进行补偿，满足特种车辆在复杂地形下自动化控制需求。

6、可选地，在本技术的一个实施例中，车身姿态传感器通过焊接方式水平安装于特种车辆的驾驶室内，车身姿态传感器与车身方向对齐。

7、可选地，在本技术的一个实施例中，gnss双天线包括主天线和副天线，gnss双天线通过焊接方式安装在特种车辆的外侧顶部，gnss双天线连线与车辆中轴线垂直，主天线和副天线的高度保持一致，主天线和副天线的间距大于预设距离。

8、可选地，在本技术的一个实施例中，核心控制器安装于特种车辆驾驶室内，核心控制器包括：rtk定位模块、位姿估计模块、地形建模与预测模块、三维地形补偿计算模块、控制模块、通讯模块，其中，

9、rtk定位模块，用于接收gnss双天线的卫星定位信息和rtk差分数据，同时获得双天线俯仰角与航向角；

10、位姿估计模块，用于使用扩展卡尔曼滤波器融合卫星定位信息与车身姿态传感器数据，实现对车辆位置和姿态的估计，获得车辆的六轴位姿；

11、地形建模与预测模块，用于通过车辆的六轴位姿以及车辆模型，对车辆所处位置地形进行建模，得到当前三维地形模型，并通过历史三维地形模型，对前方地形进行预测，得到预测三维地形模型；

12、三维地形补偿计算模块，用于基于当前和预测的三维地形模型以及车辆的速度、角速度和车辆的六轴位姿进行解算，得到车辆补偿量，其中，车辆补偿量通过车辆模型和运动模型计算得到，车辆补偿量包括横向控制补偿和位置补偿，横向控制补偿用于补偿车辆的横滚角对车辆转向轮产生扭矩，位置补偿用于补偿地形不平引起的车辆中心位置投影与轨迹的偏差；

13、控制模块，用于根据车辆补偿量对控制输出量进行补偿，结合车辆位置与预期轨迹偏差，计算控制量，通过通讯模块输出到控制器。

14、可选地，在本技术的一个实施例中，位姿估计模块，包括系统建模单元、初始化单元、预测单元、更新单元、位姿输出单元，其中，

15、系统建模单元，用于建立特种车辆的状态空间模型和状态向量，其中，状态向量包括车辆的位置、速度、姿态，姿态包括俯仰角、横滚角、航向角；

16、初始化单元，用于对状态向量和状态向量对应的状态协方差矩阵进行初始化，得到初始化值，其中，状态向量的初始化值为双天线卫星位置、航向角信息和车身姿态传感器的第一次测量值，所述协方差矩阵的初始值为随机初始化生成的值；

17、预测单元，用于在每个时间步，使用状态转移方程和控制输入预测状态向量的下一个值；

18、更新单元，用于在接收到新的双天线卫星定位信息和车身姿态传感器数据时，利用观测方程计算测量残差，并使用卡尔曼增益调整预测的状态向量，并更新状态协方差矩阵；

19、位姿输出单元，用于不断接收双天线定位信息和车身姿态传感器数据，调用预测单元和更新单元，得到实时的车身六轴位姿估计值及速度并输出。

20、可选地，在本技术的一个实施例中，对车辆所处位置地形进行建模，得到当前三维地形模型，包括：

21、获取特种车辆的轨迹点，按照预设的频率对轨迹点进行采样，得到稀疏地形点云；

22、结合车辆模型和轨迹点对应的车辆姿态，对稀疏地形点云进行扩充，得到稠密地形点云作为当前三维地形模型；

23、通过历史三维地形模型，对前方地形进行预测，得到预测三维地形模型，包括：

24、根据已经建模的地形点云，通过三维曲面拟合，预测车辆前方的地形。

25、可选地，在本技术的一个实施例中，控制量包括横向控制量和纵向控制量，控制模块，还用于：

26、将横向控制量发送给转向控制电机，使车辆的前轮按照期望的转向角/转向角速度进行转向；

27、将纵向控制量发送给油门执行器和刹车执行器，以控制发动机的功率输出和刹车力度，实现特种车辆按照目标加速度/减速度/目标速度运动。

28、可选地，在本技术的一个实施例中，还包括线缆，用于将车身姿态传感器、gnss双天线、转向控制电机和触控显示屏与核心控制器连接。

29、为达上述目的，本发明第二方面实施例提出了一种面向特种车辆应用的三维地形补偿方法，包括：接收gnss双天线的卫星定位信息和rtk差分数据，同时获得双天线俯仰角与航向角；

30、使用扩展卡尔曼滤波器融合卫星定位信息与车身姿态传感器数据，实现对车辆位置和姿态的估计，获得车辆的六轴位姿；

31、通过车辆的六轴位姿以及车辆模型，对车辆所处位置地形进行建模，得到当前三维地形模型，并通过历史三维地形模型，对前方地形进行预测，得到预测三维地形模型；

32、基于当前和预测的三维地形模型以及车辆的速度、角速度和车辆的六轴位姿进行解算，得到车辆补偿量，其中，车辆补偿量通过车辆模型和运动模型计算得到，车辆补偿量包括横向控制补偿和位置补偿，横向控制补偿用于补偿车辆的横滚角对车辆转向轮产生扭矩，位置补偿用于补偿地形不平引起的车辆中心位置投影与轨迹的偏差；

33、根据车辆补偿量对控制输出量进行补偿，结合车辆位置与预期轨迹偏差，计算控制量，通过通讯模块输出到控制器。

34、本技术附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本技术的实践了解到。