CN116588076A - 面向特种车辆应用的三维地形补偿方法和系统 - Google Patents

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

Info

Publication number
CN116588076A
CN116588076A CN202310500974.5A CN202310500974A CN116588076A CN 116588076 A CN116588076 A CN 116588076A CN 202310500974 A CN202310500974 A CN 202310500974A CN 116588076 A CN116588076 A CN 116588076A
Authority
CN
China
Prior art keywords
vehicle
terrain
control
compensation
antenna
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202310500974.5A
Other languages
English (en)
Inventor
马林
欧阳晨光
胡苏兴
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Qingbo Huachuang Measurement And Control Technology Co ltd
Original Assignee
Beijing Qingbo Huachuang Measurement And Control Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Qingbo Huachuang Measurement And Control Technology Co ltd filed Critical Beijing Qingbo Huachuang Measurement And Control Technology Co ltd
Priority to CN202310500974.5A priority Critical patent/CN116588076A/zh
Publication of CN116588076A publication Critical patent/CN116588076A/zh
Pending legal-status Critical Current

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
    • B60W30/02—Control of vehicle driving stability
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60K—ARRANGEMENT OR MOUNTING OF PROPULSION UNITS OR OF TRANSMISSIONS IN VEHICLES; ARRANGEMENT OR MOUNTING OF PLURAL DIVERSE PRIME-MOVERS IN VEHICLES; AUXILIARY DRIVES FOR VEHICLES; INSTRUMENTATION OR DASHBOARDS FOR VEHICLES; ARRANGEMENTS IN CONNECTION WITH COOLING, AIR INTAKE, GAS EXHAUST OR FUEL SUPPLY OF PROPULSION UNITS IN VEHICLES
    • B60K37/00—Dashboards
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
    • B60W30/18—Propelling the vehicle
    • B60W30/18009—Propelling the vehicle related to particular drive situations
    • B60W30/18145—Cornering
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
    • B62D—MOTOR VEHICLES; TRAILERS
    • B62D5/00—Power-assisted or power-driven steering
    • B62D5/04—Power-assisted or power-driven steering electrical, e.g. using an electric servo-motor connected to, or forming part of, the steering gear
    • B62D5/0457—Power-assisted or power-driven steering electrical, e.g. using an electric servo-motor connected to, or forming part of, the steering gear characterised by control features of the drive means as such
    • B62D5/046—Controlling the motor
    • B62D5/0463—Controlling the motor calculating assisting torque from the motor based on driver input
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
    • B60W2050/0001—Details of the control system
    • B60W2050/0019—Control system elements or transfer functions
    • B60W2050/0028—Mathematical models, e.g. for simulation
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
    • B60W2050/0001—Details of the control system
    • B60W2050/0019—Control system elements or transfer functions
    • B60W2050/0028—Mathematical models, e.g. for simulation
    • B60W2050/0031—Mathematical model of the vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Transportation (AREA)
  • Mechanical Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Chemical & Material Sciences (AREA)
  • Combustion & Propulsion (AREA)
  • Human Computer Interaction (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本申请提出了一种面向特种车辆应用的三维地形补偿系统，涉及特种车辆应用技术领域，其中，该方法包括：车身姿态传感器，用于检测特种车辆的车身姿态；GNSS双天线，用于接收特种车辆的卫星定位信息；转向控制电机，用于控制特种车辆转向；触控显示屏，用于显示特种车辆的状态信息；核心控制器，用于根据特种车辆的车身姿态、卫星定位信息控制特种车辆运动，通过转向控制电机控制特种车辆转向，并将特种车辆的状态信息显示在触控显示屏上。采用上述方案的本发明能够满足满足特种车辆在复杂地形下自动化控制需求。

Description

面向特种车辆应用的三维地形补偿方法和系统
技术领域
本申请涉及特种车辆应用技术领域，尤其涉及面向特种车辆应用的三维地形补偿方法和系统。
背景技术
特种车辆广泛应用于工程施工、农业作业、消防安全以及国防军事等领域，其工作环境往往比较复杂，对于工作效率要求较高，有着自动化和智能化需求。由于特种车辆工作的地形经常起伏不平或者存在较大坡度，对于其自动化控制系统来说存在相当大的挑战，传统的自动化控制系统在复杂地形下容易出现不稳定或者失效的情况，导致较大的作业偏差甚至对行驶安全造成威胁。
发明内容
本申请旨在至少在一定程度上解决相关技术中的技术问题之一。
为此，本申请的第一个目的在于提出一种面向特种车辆应用的三维地形补偿系统，解决了现有方法在复杂地形下容易出现不稳定或者失效的情况，导致较大的作业偏差甚至对行驶安全造成威胁的技术问题，满足了特种车辆在复杂地形下自动化控制需求。
本申请的第二个目的在于提出一种面向特种车辆应用的三维地形补偿系统。
为达上述目的，本申请第一方面实施例提出了一种面向特种车辆应用的三维地形补偿系统，包括：车身姿态传感器，用于检测特种车辆的车身姿态；GNSS双天线，用于接收特种车辆的卫星定位信息；转向控制电机，用于控制特种车辆转向；触控显示屏，用于显示特种车辆的状态信息；核心控制器，用于根据特种车辆的车身姿态、卫星定位信息控制特种车辆运动，通过转向控制电机控制特种车辆转向，并将特种车辆的状态信息显示在触控显示屏上。
本申请实施例的面向特种车辆应用的三维地形补偿系统，通过车身姿态传感器与GNSS双天线融合进行车辆位姿估计，并对三维地形进行建模与预测，根据地形对控制量进行补偿，满足特种车辆在复杂地形下自动化控制需求。
可选地，在本申请的一个实施例中，车身姿态传感器通过焊接方式水平安装于特种车辆的驾驶室内，车身姿态传感器与车身方向对齐。
可选地，在本申请的一个实施例中，GNSS双天线包括主天线和副天线，GNSS双天线通过焊接方式安装在特种车辆的外侧顶部，GNSS双天线连线与车辆中轴线垂直，主天线和副天线的高度保持一致，主天线和副天线的间距大于预设距离。
可选地，在本申请的一个实施例中，核心控制器安装于特种车辆驾驶室内，核心控制器包括：RTK定位模块、位姿估计模块、地形建模与预测模块、三维地形补偿计算模块、控制模块、通讯模块，其中，
RTK定位模块，用于接收GNSS双天线的卫星定位信息和RTK差分数据，同时获得双天线俯仰角与航向角；
位姿估计模块，用于使用扩展卡尔曼滤波器融合卫星定位信息与车身姿态传感器数据，实现对车辆位置和姿态的估计，获得车辆的六轴位姿；
地形建模与预测模块，用于通过车辆的六轴位姿以及车辆模型，对车辆所处位置地形进行建模，得到当前三维地形模型，并通过历史三维地形模型，对前方地形进行预测，得到预测三维地形模型；
三维地形补偿计算模块，用于基于当前和预测的三维地形模型以及车辆的速度、角速度和车辆的六轴位姿进行解算，得到车辆补偿量，其中，车辆补偿量通过车辆模型和运动模型计算得到，车辆补偿量包括横向控制补偿和位置补偿，横向控制补偿用于补偿车辆的横滚角对车辆转向轮产生扭矩，位置补偿用于补偿地形不平引起的车辆中心位置投影与轨迹的偏差；
控制模块，用于根据车辆补偿量对控制输出量进行补偿，结合车辆位置与预期轨迹偏差，计算控制量，通过通讯模块输出到控制器。
可选地，在本申请的一个实施例中，位姿估计模块，包括系统建模单元、初始化单元、预测单元、更新单元、位姿输出单元，其中，
系统建模单元，用于建立特种车辆的状态空间模型和状态向量，其中，状态向量包括车辆的位置、速度、姿态，姿态包括俯仰角、横滚角、航向角；
初始化单元，用于对状态向量和状态向量对应的状态协方差矩阵进行初始化，得到初始化值，其中，状态向量的初始化值为双天线卫星位置、航向角信息和车身姿态传感器的第一次测量值，所述协方差矩阵的初始值为随机初始化生成的值；
预测单元，用于在每个时间步，使用状态转移方程和控制输入预测状态向量的下一个值；
更新单元，用于在接收到新的双天线卫星定位信息和车身姿态传感器数据时，利用观测方程计算测量残差，并使用卡尔曼增益调整预测的状态向量，并更新状态协方差矩阵；
位姿输出单元，用于不断接收双天线定位信息和车身姿态传感器数据，调用预测单元和更新单元，得到实时的车身六轴位姿估计值及速度并输出。
可选地，在本申请的一个实施例中，对车辆所处位置地形进行建模，得到当前三维地形模型，包括：
获取特种车辆的轨迹点，按照预设的频率对轨迹点进行采样，得到稀疏地形点云；
结合车辆模型和轨迹点对应的车辆姿态，对稀疏地形点云进行扩充，得到稠密地形点云作为当前三维地形模型；
通过历史三维地形模型，对前方地形进行预测，得到预测三维地形模型，包括：
根据已经建模的地形点云，通过三维曲面拟合，预测车辆前方的地形。
可选地，在本申请的一个实施例中，控制量包括横向控制量和纵向控制量，控制模块，还用于：
将横向控制量发送给转向控制电机，使车辆的前轮按照期望的转向角/转向角速度进行转向；
将纵向控制量发送给油门执行器和刹车执行器，以控制发动机的功率输出和刹车力度，实现特种车辆按照目标加速度/减速度/目标速度运动。
可选地，在本申请的一个实施例中，还包括线缆，用于将车身姿态传感器、GNSS双天线、转向控制电机和触控显示屏与核心控制器连接。
为达上述目的，本发明第二方面实施例提出了一种面向特种车辆应用的三维地形补偿方法，包括：接收GNSS双天线的卫星定位信息和RTK差分数据，同时获得双天线俯仰角与航向角；
使用扩展卡尔曼滤波器融合卫星定位信息与车身姿态传感器数据，实现对车辆位置和姿态的估计，获得车辆的六轴位姿；
通过车辆的六轴位姿以及车辆模型，对车辆所处位置地形进行建模，得到当前三维地形模型，并通过历史三维地形模型，对前方地形进行预测，得到预测三维地形模型；
基于当前和预测的三维地形模型以及车辆的速度、角速度和车辆的六轴位姿进行解算，得到车辆补偿量，其中，车辆补偿量通过车辆模型和运动模型计算得到，车辆补偿量包括横向控制补偿和位置补偿，横向控制补偿用于补偿车辆的横滚角对车辆转向轮产生扭矩，位置补偿用于补偿地形不平引起的车辆中心位置投影与轨迹的偏差；
根据车辆补偿量对控制输出量进行补偿，结合车辆位置与预期轨迹偏差，计算控制量，通过通讯模块输出到控制器。
本申请附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本申请的实践了解到。
附图说明
本申请上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中：
图1为本申请实施例一所提供的一种面向特种车辆应用的三维地形补偿系统的结构示意图。
具体实施方式
下面详细描述本申请的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本申请，而不能理解为对本申请的限制。
下面参考附图描述本申请实施例的面向特种车辆应用的三维地形补偿方法和系统。
图1为本申请实施例一所提供的一种面向特种车辆应用的三维地形补偿系统的结构示意图。
如图1所示，该面向特种车辆应用的三维地形补偿系统：
车身姿态传感器2，用于检测特种车辆的车身姿态；
GNSS双天线1，用于接收特种车辆的卫星定位信息；
转向控制电机3，用于控制特种车辆转向；
触控显示屏4，用于显示特种车辆的状态信息；
核心控制器12，用于根据特种车辆的车身姿态、卫星定位信息控制特种车辆运动，通过转向控制电机3控制特种车辆转向，并将特种车辆的状态信息显示在触控显示屏4上。
本申请实施例的面向特种车辆应用的三维地形补偿系统，通过车身姿态传感器2与GNSS双天线1融合进行车辆位姿估计，并对三维地形进行建模与预测，根据地形对控制量进行补偿，满足特种车辆在复杂地形下自动化控制需求。
可选地，在本申请的一个实施例中，车身姿态传感器2通过焊接方式水平安装于特种车辆的驾驶室内，车身姿态传感器2与车身方向对齐。
可选地，在本申请的一个实施例中，GNSS双天线1包括主天线和副天线，GNSS双天线1通过焊接方式安装在特种车辆的外侧顶部，GNSS双天线连线与车辆中轴线垂直，主天线和副天线的高度保持一致，主天线和副天线的间距在1m以上。
可选地，在本申请的一个实施例中，核心控制器12安装于特种车辆驾驶室内，通过车辆电源供电，电压范围为9-36V，
核心控制器12包括：RTK定位模块8、位姿估计模块9、地形建模与预测模块10、三维地形补偿计算模块11、控制模块6、通讯模块7，其中，
RTK定位模块8，用于通过接收GNSS双天线1的卫星定位信息以及RTK差分数据获得厘米级定位精度，同时可获得双天线俯仰角与航向角；；
位姿估计模块9，用于融合双天线定位信息与车身姿态传感器数据，对车辆位置和姿态进行估计，采用的融合算法为扩展卡尔曼滤波器，获得车辆的六轴位姿，包括东北高三个坐标以及俯仰角、横滚角与偏航角三个角度；
融合定位过程包括系统建模、初始化、预测、更新。首先，需要建立一个状态空间模型。状态向量包括车辆的位置(x，y，z坐标)、速度(Vx，Vy，Vz)、姿态(俯仰角，横滚角，航向角)。初始化的目的是为状态向量和协方差矩阵提供一个初始估计，对于状态向量，使用双天线卫星位置和航向角信息和车身姿态传感器的第一次测量值，对于协方差矩阵，可以选择一个较大的初始值来表示初始不确定性。预测是指在每个时间步，使用状态转移方程和控制输入(如加速度)来预测状态向量的下一个值，同时更新状态协方差矩阵以表示预测不确定性。更新是当接收到新的双天线卫星定位信息和车身姿态传感器数据时，利用观测方程来计算测量残差，然后使用卡尔曼增益来调整预测的状态向量，使其更接近实际值，同时更新状态协方差矩阵以表示估计不确定性。成功初始化后，不断接收双天线定位信息和车身姿态传感器数据，重复预测和更新步骤，输出实时的车身六轴位姿估计值以及速度。
地形建模与预测模块10，用于通过车辆的六轴位姿以及车辆模型，对车辆所处位置地形进行建模，得到当前三维地形模型，并通过历史三维地形模型，对前方地形进行预测，得到预测三维地形模型；
地形建模需要对车辆历史轨迹点的三维位置和车辆姿态进行存储，轨迹点按照一定的频率进行采样，得到稀疏的地形点云，结合车辆模型和轨迹点对应的车辆姿态，对轨迹点云进行扩充，得到更加稠密的地形。根据已经建模的地形点云，通过三维曲面拟合，可以预测车辆前方的地形。
三维地形补偿计算模块11，用于对基于当前和预测的三维地形以及车辆的速度、角速度和姿态信息进行解算，一方面，在横滚方向存在侧滑力，需要计算横向控制补偿项，另一方面，横滚和俯仰方向的坡度会导致车辆中心投影点不在后轮中心线上，对于高精度轨迹控制来说造成不可忽略的误差，需要进行位置补偿；
其中，横向控制补偿是补偿车辆的横滚角对车辆转向轮产生扭矩，位置补偿是补偿地形不平引起的车辆中心位置投影与轨迹的偏差。补偿量的计算通过车辆模型和运动模型获得，补偿项是一个比例-积分-微分(PID)控制器，这个补偿项将用于调整车辆的转向角，提高复杂地形下的车辆控制精度。
控制模块6，用于接收三维地形补偿计算模块输出的补偿量，对控制输出量进行补偿，结合车辆位置与预期轨迹偏差，计算控制量，输出到控制模块。
控制量分为横向和纵向控制量，横向控制的目标是使车辆沿预定轨迹行驶，横向控制量输出是期望的转向角或转向角速度，系统将这个控制量发送给转向控制电机，使车辆的前轮按照期望的转向角进行转向。纵向控制的目标是控制车辆的速度和加速度，纵向控制量输出可以是期望的加速度、减速度或目标速度，系统将这个控制量发送给油门执行器和刹车执行器，以控制发动机的功率输出和刹车力度。
可选地，在本申请的一个实施例中，还包括线缆5，用于将车身姿态传感器2、GNSS双天线1、转向控制电机3和触控显示屏4与核心控制器12连接。
本申请提出的面向特种车辆应用的三维地形补偿系统的所有模块安装完成后，首先需要进行参数标定，获取车辆模型参数，并对传感器安装误差进行补偿。
当系统开始工作时，GNSS双天线获取卫星定位信息，通过RTK定位模块进行解算获取高精度定位数据以及双天线航向角和横滚角数据；车身姿态传感器获取车辆的三轴姿态角度以及角速度数据；数据通过通讯模块进行传输。
姿态估计模块获取RTK定位模块数据和车身姿态传感器数据，通过扩展卡尔曼滤波器算法进行传感器数据融合，获得高精度车辆位姿结果；地形建模与预测模块接收位姿估计结果，对特种车辆当前位置的地形进行建模，并保存当前地形数据，另外还会预测前方地形，输出给三维地形补偿计算模块；
三维地形补偿计算模块根据地形建模结果，结合位姿估计结果，计算两项补偿项，一项是由横滚角引起的侧滑力，需要对横向控制进行补偿；另一项是横滚角和俯仰角造成的投影位置误差，需要对位置进行补偿；
控制模块接收上述各模块的数据，计算补偿后的控制量输出给转向控制电机，从而实现带用三维地形补偿的特种车辆自动控制。
本系统过车身姿态传感器与GNSS双天线融合获得更加准确的车身位姿估计结果，实时对地形进行建模和预测，并保存历史轨迹地形数据，结合三维地形数据计算车辆控制量补偿项，考虑横滚和俯仰角度引起的侧滑力与位置误差，提高特种车辆在复杂地形下自动控制的稳定性和精度，提高其工作效率与安全性。
为了实现上述实施例，本申请还提出一种面向特种车辆应用的三维地形补偿方法。
该面向特种车辆应用的三维地形补偿方法包括：接收GNSS双天线的卫星定位信息和RTK差分数据，同时获得双天线俯仰角与航向角；
使用扩展卡尔曼滤波器融合卫星定位信息与车身姿态传感器数据，实现对车辆位置和姿态的估计，获得车辆的六轴位姿；
通过车辆的六轴位姿以及车辆模型，对车辆所处位置地形进行建模，得到当前三维地形模型，并通过历史三维地形模型，对前方地形进行预测，得到预测三维地形模型；
基于当前和预测的三维地形模型以及车辆的速度、角速度和车辆的六轴位姿进行解算，得到车辆补偿量，其中，解算包括横向控制补偿项解算和位置补偿解算；
根据车辆补偿量对控制输出量进行补偿，结合车辆位置与预期轨迹偏差，计算控制量，通过通讯模块输出到控制器。
需要说明的是，前述对面向特种车辆应用的三维地形补偿系统实施例的解释说明也适用于该实施例的面向特种车辆应用的三维地形补偿方法，此处不再赘述。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本申请的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本申请的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。
流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现定制逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本申请的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本申请的实施例所属技术领域的技术人员所理解。
在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。就本说明书而言，"计算机可读介质"可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。计算机可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(RAM)，只读存储器(ROM)，可擦除可编辑只读存储器(EPROM或闪速存储器)，光纤装置，以及便携式光盘只读存储器(CDROM)。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。
应当理解，本申请的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。如，如果用硬件来实现和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。
本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。
此外，在本申请各个实施例中的各功能单元可以集成在一个处理模块中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。
上述提到的存储介质可以是只读存储器，磁盘或光盘等。尽管上面已经示出和描述了本申请的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本申请的限制，本领域的普通技术人员在本申请的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (9)

1.一种面向特种车辆应用的三维地形补偿系统，其特征在于，包括：
车身姿态传感器，用于检测所述特种车辆的车身姿态；
GNSS双天线，用于接收所述特种车辆的卫星定位信息；
转向控制电机，用于控制所述特种车辆转向；
触控显示屏，用于显示所述特种车辆的状态信息；
核心控制器，用于根据所述特种车辆的车身姿态、卫星定位信息控制所述特种车辆运动，通过所述转向控制电机控制所述特种车辆转向，并将所述特种车辆的状态信息显示在所述触控显示屏上。
2.如权利要求1所述的系统，其特征在于，所述车身姿态传感器通过焊接方式水平安装于所述特种车辆的驾驶室内，所述车身姿态传感器与车身方向对齐。
3.如权利要求1所述的系统，其特征在于，所述GNSS双天线包括主天线和副天线，所述GNSS双天线通过焊接方式安装在所述特种车辆的外侧顶部，所述GNSS双天线连线与车辆中轴线垂直，所述主天线和副天线的高度保持一致，所述主天线和副天线的间距大于预设距离。
4.如权利要求1所述的系统，其特征在于，所述核心控制器安装于所述特种车辆驾驶室内，所述核心控制器包括：RTK定位模块、位姿估计模块、地形建模与预测模块、三维地形补偿计算模块、控制模块、通讯模块，其中，
所述RTK定位模块，用于接收GNSS双天线的卫星定位信息和RTK差分数据，同时获得双天线俯仰角与航向角；
所述位姿估计模块，用于使用扩展卡尔曼滤波器融合所述卫星定位信息与车身姿态传感器数据，实现对车辆位置和姿态的估计，获得车辆的六轴位姿；
所述地形建模与预测模块，用于通过所述车辆的六轴位姿以及车辆模型，对车辆所处位置地形进行建模，得到当前三维地形模型，并通过历史三维地形模型，对前方地形进行预测，得到预测三维地形模型；
所述三维地形补偿计算模块，用于基于当前和预测的三维地形模型以及车辆的速度、角速度和所述车辆的六轴位姿进行解算，得到车辆补偿量，其中，所述车辆补偿量通过车辆模型和运动模型计算得到，所述车辆补偿量包括横向控制补偿和位置补偿，所述横向控制补偿用于补偿车辆的横滚角对车辆转向轮产生扭矩，所述位置补偿用于补偿地形不平引起的车辆中心位置投影与轨迹的偏差；
所述控制模块，用于根据所述车辆补偿量对控制输出量进行补偿，结合车辆位置与预期轨迹偏差，计算控制量，通过所述通讯模块输出到控制器。
5.如权利要求4所述的系统，其特征在于，所述位姿估计模块，包括系统建模单元、初始化单元、预测单元、更新单元、位姿输出单元，其中，
所述系统建模单元，用于建立所述特种车辆的状态空间模型和状态向量，其中，所述状态向量包括车辆的位置、速度、姿态，所述姿态包括俯仰角、横滚角、航向角；
所述初始化单元，用于对所述状态向量和所述状态向量对应的状态协方差矩阵进行初始化，得到初始化值，其中，所述状态向量的初始化值为双天线卫星位置、航向角信息和车身姿态传感器的第一次测量值，所述协方差矩阵的初始值为随机初始化生成的值；
所述预测单元，用于在每个时间步，使用状态转移方程和控制输入预测状态向量的下一个值；
所述更新单元，用于在接收到新的双天线卫星定位信息和车身姿态传感器数据时，利用观测方程计算测量残差，并使用卡尔曼增益调整预测的状态向量，并更新所述状态协方差矩阵；
所述位姿输出单元，用于不断接收双天线定位信息和车身姿态传感器数据，调用所述预测单元和所述更新单元，得到实时的车身六轴位姿估计值及速度并输出。
6.如权利要求4所述的系统，其特征在于，所述对车辆所处位置地形进行建模，得到当前三维地形模型，包括：
获取所述特种车辆的轨迹点，按照预设的频率对所述轨迹点进行采样，得到稀疏地形点云；
结合所述车辆模型和轨迹点对应的车辆姿态，对所述稀疏地形点云进行扩充，得到稠密地形点云作为所述当前三维地形模型；
所述通过历史三维地形模型，对前方地形进行预测，得到预测三维地形模型，包括：
根据已经建模的地形点云，通过三维曲面拟合，预测车辆前方的地形。
7.如权利要求4所述的系统，其特征在于，所述控制量包括横向控制量和纵向控制量，所述控制模块，还用于：
将所述横向控制量发送给所述转向控制电机，使车辆的前轮按照期望的转向角/转向角速度进行转向；
将所述纵向控制量发送给油门执行器和刹车执行器，以控制发动机的功率输出和刹车力度，实现所述特种车辆按照目标加速度/减速度/目标速度运动。
8.如权利要求1所述的系统，其特征在于，还包括线缆，用于将所述车身姿态传感器、GNSS双天线、转向控制电机和触控显示屏与核心控制器连接。
9.一种面向特种车辆应用的三维地形补偿方法，其特征在于，所述方法由权利要求1中的核心控制器实现，所述方法包括：
接收GNSS双天线的卫星定位信息和RTK差分数据，同时获得双天线俯仰角与航向角；
使用扩展卡尔曼滤波器融合所述卫星定位信息与车身姿态传感器数据，实现对车辆位置和姿态的估计，获得车辆的六轴位姿；
通过所述车辆的六轴位姿以及车辆模型，对车辆所处位置地形进行建模，得到当前三维地形模型，并通过历史三维地形模型，对前方地形进行预测，得到预测三维地形模型；
基于当前和预测的三维地形模型以及车辆的速度、角速度和所述车辆的六轴位姿进行解算，得到车辆补偿量，其中，所述车辆补偿量通过车辆模型和运动模型计算得到，所述车辆补偿量包括横向控制补偿和位置补偿，所述横向控制补偿用于补偿车辆的横滚角对车辆转向轮产生扭矩，所述位置补偿用于补偿地形不平引起的车辆中心位置投影与轨迹的偏差；
根据所述车辆补偿量对控制输出量进行补偿，结合车辆位置与预期轨迹偏差，计算控制量，通过所述通讯模块输出到控制器。
CN202310500974.5A 2023-05-05 2023-05-05 面向特种车辆应用的三维地形补偿方法和系统 Pending CN116588076A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310500974.5A CN116588076A (zh) 2023-05-05 2023-05-05 面向特种车辆应用的三维地形补偿方法和系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310500974.5A CN116588076A (zh) 2023-05-05 2023-05-05 面向特种车辆应用的三维地形补偿方法和系统

Publications (1)

Publication Number Publication Date
CN116588076A true CN116588076A (zh) 2023-08-15

Family

ID=87589045

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310500974.5A Pending CN116588076A (zh) 2023-05-05 2023-05-05 面向特种车辆应用的三维地形补偿方法和系统

Country Status (1)

Country Link
CN (1) CN116588076A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117040608A (zh) * 2023-10-10 2023-11-10 四川轻化工大学 一种车载卫星中继

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117040608A (zh) * 2023-10-10 2023-11-10 四川轻化工大学 一种车载卫星中继
CN117040608B (zh) * 2023-10-10 2023-12-08 四川轻化工大学 一种车载卫星中继

Similar Documents

Publication Publication Date Title
US11198341B2 (en) Trailer detection and autonomous hitching
CN110262495B (zh) 可实现移动机器人自主导航与精确定位的控制系统及方法
CN109215370B (zh) 用于计算适于道路车辆的最优停车操纵的路径规划方法和相应系统
US10310503B2 (en) Travel control apparatus for vehicle
CN106476728B (zh) 用于车载的车辆传感器的运动补偿
EP2460059B1 (en) Estimating positions of a device and at least one target in an environment
JP7036080B2 (ja) 慣性航法装置
US20150346723A1 (en) Vehicle trajectory planning for autonomous vehicles
CN107085424A (zh) 用于自动驾驶的预瞄横向控制
JP6473684B2 (ja) 車輪の滑り角推定装置及びその方法
KR20110124646A (ko) 자율이동차량의 조향제어장치, 이를 구비하는 자율이동차량 및 자율이동차량의 조향제어방법
CN116588076A (zh) 面向特种车辆应用的三维地形补偿方法和系统
CN113433947A (zh) 基于障碍车估计与预测的交叉路口轨迹规划与控制方法
CN112896147A (zh) 用于矿用车辆的双向定位泊车控制方法及装置
JP2021071734A (ja) 移動体の制御装置及び移動体の制御方法
Sahoo et al. Design and development of a heading angle controller for an unmanned ground vehicle
Hurni et al. A pseudospectral optimal motion planner for autonomous unmanned vehicles
Zhu et al. Global positioning system-based vehicle control for automated parking
US10723345B2 (en) Horizon-stabilized lateral control for a vehicle
EP2287698A1 (en) Estimating positions of a device and at least one target in an environment
Nejad et al. Real time localization, path planning and motion control for autonomous parking in cluttered environment with narrow passages
Wang et al. Accurate Perception for Autonomous Driving: Application of Kalman Filter for Sensor Fusion
Yan et al. Model predictive control for unmanned tracked vehicle path following
Lucet et al. A kinematic parameter estimator applied to a bi-directional vehicle
Khablov Development of a Path-Tracking Control System Model for an Indoor Mobile Robot Using Odometers and Microwave Sensors

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination