CN112987069A - 一种基于车***姿的作业部件末端位姿测量方法 - Google Patents

一种基于车***姿的作业部件末端位姿测量方法 Download PDF

Info

Publication number
CN112987069A
CN112987069A CN202110188180.0A CN202110188180A CN112987069A CN 112987069 A CN112987069 A CN 112987069A CN 202110188180 A CN202110188180 A CN 202110188180A CN 112987069 A CN112987069 A CN 112987069A
Authority
CN
China
Prior art keywords
vehicle body
attitude
pose
tail end
information
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.)
Granted
Application number
CN202110188180.0A
Other languages
English (en)
Other versions
CN112987069B (zh
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.)
South China Agricultural University
Original Assignee
South China Agricultural University
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 South China Agricultural University filed Critical South China Agricultural University
Priority to CN202110188180.0A priority Critical patent/CN112987069B/zh
Publication of CN112987069A publication Critical patent/CN112987069A/zh
Application granted granted Critical
Publication of CN112987069B publication Critical patent/CN112987069B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/53—Determining attitude

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

一种基于车***姿的作业部件末端位姿测量方法，a.将姿态传感器安装于车体或载体平台测量车体姿态；b.GNSS***测量车体的位姿信息；激光传感测量基于基准作业面的相对位置信息；c.建立车体坐标系Oxyz；d.执行机构安装传感器，检测姿态及位置变化，并建立作业部件坐标系Oxnynzn；e.获取卫星天线或激光传感的位置信息和各传感器所测姿态及位置信息，建立作业部件末端中心的位置解算模型；f.计算作业部件末端基于基准作业面的高程差Δh；测量作业部件的姿态信息，计算作业部件末端基于基准作业面的角度差Δα。使用一套GNSS***便可车体定位和作业部件末端位姿测量，属于工程机械和智能农机领域。

Description

一种基于车***姿的作业部件末端位姿测量方法
技术领域
本发明涉及工程机械和智能农机领域，特别涉及一种基于车***姿的作业部件末端位姿测量方法。
背景技术
随着科学技术的不断发展，人们对机械的作业精度与作业效率的要求不断提高，这要求对末端作业部件位姿信息的准确感知。随着卫星定位***载波相位差分技术的快速发展，利用卫星定位来测量载体的姿态信息迅速成为卫星定位应用研究的热点。GNSS的位姿信息有着精度高、无误差累计、无需初始化和成本低等优势。激光技术是20世纪以来最重要的科技发明之一，因其准直性强、相干性好、物理性质稳定、精度高等特性，使其应用领域越来越广泛。激光平整技术以旋转激光作为校准平面，激光接收器根据不同高程的光电转换元件接收到的激光信号确定所安装载体的高程位置。
近年来，在国内，华南农业大学设计基于GNSS技术的水田平地机，将GNSS接收天线固定在平地铲上以获取平地铲的高程定位信息(胡炼等，2015)；该团队还设计了高精度的激光平地机激光接收器，应用于农田平整与混泥土平整(蒙世博等，2020)。在国外，天宝(Trimble)Field LevelⅡSystem平地机与徕卡的Leica iCON grade iGD系列推土机将GNSS双天线安装于平地铲/推土铲上以得到作业部件的位置及姿态信息。
顺应无人作业的发展趋势，利用多套GNSS实现自动导航及作业部件姿态与高程位置信息的测量，不仅增加了成本，而且在作业部件末端安装GNSS天线难度大、测量不准，甚至无法安装，如需前后翻转的作业部件。此外，作业部件上的桅杆、电缆和天线一定程度上限制了作业部件的运行范围，增加了设备的损坏风险，同时也增加了每天拆卸和重装所需的时间。因此有必要提供一种仅用一套GNSS***或激光传感便可满足车体定位和作业部件末端位姿测量的方法。更为重要的是，复杂造型施工和水下作业等要求测量快速获得高精度的作业部件末端的位置信息，而GNSS***和激光传感难以直接测量获得。
因此，有必要提供一种基于车***姿的作业部件末端位姿测量方法以满足应用需求。
发明内容
针对现有技术中存在的技术问题，本发明的目的是：提供一种基于车***姿的作业部件末端位姿测量方法，使用一套GNSS***或激光传感便可满足车体定位和作业部件末端位姿测量要求。
为了达到上述目的，本发明采用如下技术方案：
一种基于车***姿的作业部件末端位姿测量方法，包括如下步骤：
a、将姿态传感器安装于车体或与车体刚性连接的载体平台上，测量车体姿态；
b、将GNSS***中的卫星天线安装于车体顶部，测量车体的位姿信息；或将激光传感安装于车体或载体平台上，测量基于基准作业面的相对位置信息；
c、建立车体坐标系Oxyz，以车体前进方向为y轴，x轴与y轴垂直指向车体方向的右侧，z轴垂直于x、y轴向上；
d、在各个执行机构安装传感器，检测执行机构的姿态变化以及位置变化，并建立作业部件坐标系Oxnynzn，n＝1,2...；
e、获取卫星天线或激光传感的位置信息，获取车体姿态信息，获取各执行机构的姿态信息及位置变化量，建立基于所获取信息及执行机构几何关系求解作业部件末端中心的位置解算模型，并计算获得作业部件末端中心坐标；
f、测量作业部件的姿态信息并推算作业部件末端的姿态信息；
g、设计基准作业面，结合所求的作业部件末端位姿信息计算作业部件末端与基准作业面的高程差Δh，以及作业部件末端与基准作业面的角度差Δα。
作为一种优选，步骤b中，GNSS***用于计算偏航角ψ及空间位置坐标。该条适用于GNSS***。
作为一种优选，步骤d中，根据执行机构的运动需求，传感器选择姿态传感器或位移传感器。该条适用于GNSS与激光传感***。
作为一种优选，步骤e中，根据卫星天线的位置信息、车体姿态信息和各执行机构的姿态信息及位置变化量，建立GNSS坐标解算模型，解算作业部件末端三维坐标，GNSS坐标解算模型如下：
Figure BDA0002943448390000021
其中，n＝1,2,...；r表示作业部件末端中心的三维坐标；p表示卫星天线的位置信息；ψ,
Figure BDA0002943448390000022
θ分别为车体偏航角、横滚角和俯仰角；β1,β2,......,βn为各执行机构上姿态传感器测量值；l1,l2,......,ln为各执行机构上位移传感器测量值。该条适用于GNSS***。
作为一种优选，解算作业部件末端三维坐标步骤如下：
(1)测量车体及执行机构的角度与长度信息和卫星天线至车体质心位置的坐标增量，输入GNSS坐标解算模型；
(2)换算车体姿态角；
(3)计算车身姿态旋转矩阵；
(4)根据几何关系或机器人运动学及所测量的各执行机构姿态信息与位置变化量，解算基于车体坐标系的作业部件末端中心的位置信息；
(5)计算卫星天线至作业部件末端中心的坐标增量；
(6)结合步骤(5)及卫星天线的位置信息，计算作业部件末端中心的三维坐标。
作为一种优选，步骤e中，根据激光传感的位置信息、车体姿态信息和各执行机构的姿态信息及位置变化量，建立激光高程解算模型，求解作业部件末端中心的高程信息，激光高程解算模型如下：
Figure BDA0002943448390000031
其中，n＝1,2,...；H表示作业部件末端中心相对于基准作业面的相对位置信息；hp表示激光接收器相对于基准作业面的相对高程；ψ,
Figure BDA0002943448390000032
θ分别为车体偏航角、横滚角和俯仰角；β1,β2,......,βn为各执行机构上姿态传感器测量值；l1,l2,......,ln为各执行机构上位移传感器测量值。该条适用于激光传感***。
作为一种优选，作业部件末端中心高程解算解算步骤如下：
(1)测量车体及执行机构的角度与长度信息和激光接收器至车体或载体平台姿态传感器安装位置的Z轴坐标增量，输入激光高程解算模型；
(2)计算车体姿态旋转矩阵；
(3)根据几何关系或机器人运动学及所测量的各执行机构姿态信息与位置变化量，解算基于车体坐标系的作业部件末端中心的坐标Z轴分量；
(4)计算激光接收器至作业部件末端中心的Z轴坐标增量；
(5)结合步骤(4)及激光传感的位置信息，计算作业部件末端中心相对于基准作业面的相对位置信息。
作为一种优选，步骤g中，设计基准作业面包括高程h，基于所求作业部件末端的三维坐标的Z轴分量，计算作业部件末端在基准作业面各位置处的高程差；在激光传感应用上，基准作业面为平面的高程，激光接收器与激光发射器形成相对高程差，在设计基准作业面(即设计激光发射器位置)的同时，可知激光接收器与基准作业面的高程差。该条适用于GNSS与激光传感***。
作为一种优选，步骤g中，设计基准作业面包括设计基准作业面角度α，取与基准作业面相关的作业部件末端姿态，计算作业部件末端基于基准作业面的角度差。
作为一种优选，车体姿态角中的初始偏航角ψ，是由卫星信号接收模块从卫星天线的位置关系数据中经滤波得到或与姿态传感器所测偏航角融合得到；用于计算的卫星天线位置信息由天线初始信息经滤波再进行坐标转换得到。该条适用于GNSS***。
本发明的原理是：利用姿态传感器或融合姿态传感器与GNSS***检测车体的姿态(航向角、俯仰角和横滚角)，采用传感器测量各执行机构的姿态信息及位置变化量，结合卫星天线所测位置信息，建立解算作业部件末端中心实时位置的GNSS坐标解算模型；在激光传感***中，利用姿态传感器测量车体姿态，采用传感器测量各执行机构的姿态信息及位置变化量，结合激光传感所测位置信息，建立解算作业部件末端中心实时位置的激光高程解算模型；并基于基准作业面解算作业部件的实时姿态及作业部件末端与基准作业面的实时高程差。相比传统人工测量的工作方式，作业部件末端位置与姿态的解算具有精度高、安全高性能高、速度快，同时可满足施工造型复杂与水下作业的要求，减少了人力成本和劳动强度。保证了作业机具的精准，以达到精确作业的目的。
本发明与现有技术相比具有如下优点和效果：
(1)本发明仅用一套GNSS***便可满足车体定位及作业部件末端位姿测量要求。
(2)该方法不仅可获得作业部件末端的位置信息还可获得作业部件末端的姿态信息。相比传统人工测量的工作方式，作业部件末端位置三维坐标与姿态的解算具有精度高、安全性能高、速度快，同时可满足施工造型复杂与水下作业的要求，减少了人力成本和劳动强度。
(3)本发明通过GNSS***和姿态传感器检测装备车体的姿态角度(航向角、俯仰角和横滚角)，采用传感器测量各执行机构的姿态信息及位置变化量，结合卫星天线所测位置信息，建立解算作业部件末端实时位置的GNSS坐标解算模型；无需定位场合，利用姿态传感器测量车体姿态，采用传感器测量各执行机构的姿态信息及位置变化量，结合激光传感所测位置信息，建立解算作业部件末端中心实时位置的激光高程解算模型；并基于设计基准作业面解算作业机具的实时姿态及作业部件末端与基准作业面的实时高程差。保证了作业机具的精准，以达到精确作业的目的。
(4)本发明可解决作业部件运动复杂以及作业部件末端不方便安装GNSS天线或激光高程传感器的而无法测量问题。将GNSS或激光设备从作业部件末端移至车体或载体平台上，减少了每天拆卸和重装所需要的时间，也降低了设备的损坏风险。
附图说明
图1为采用GNSS***的基于车***姿的作业部件末端位姿测量方法的流程图。
图2为采用激光传感的基于车***姿的作业部件末端位姿测量方法的流程图。
图3a为整平机的结构示意图。
图3b为整平机的平地铲的运动轨迹图。
图4a为平地机的结构示意图。
图4b为平地机的计算简图。
图3a和图3b中，1-1为装备车体，1-2为激光接收器，1-3为激光发射器，1-4为姿态传感器，1-5为高程油缸，1-6为平地铲，1-7为蒲滚，1-8为位移传感器。
图4a和图4b中，2-1为装备车体，2-2为卫星天线，2-4为液压***，2-5为高程油缸，2-6为水平油缸，2-7为平地铲水平姿态传感器，2-8为平地铲，2-3、2-9和2-10为姿态传感器。
具体实施方式
下面将结合具体实施方式来对本发明做进一步详细的说明。
实施例一
本发明一种基于车***姿的作业部件末端位姿测量方法，用于整平机，如图3a和3b所示，该实施例主要简述应用激光传感计算作业部件末端基于基准作业平面的高程差Δh。
本实施例采用一马整平机，包括：装备车体1-1、激光接收器1-2、激光发射器1-3、姿态传感器1-4、高程油缸1-5、平地铲1-6(本实施例中的作业部件末端)、蒲滚1-7(载体平台)、位移传感器1-8。平地铲与蒲滚连接，可通过高程油缸实现平地铲的高程调节，位移传感器安装在高程油缸上，测量油缸行程。蒲滚经三点悬挂挂接在车体上，将姿态传感器安装在蒲滚上，间接测量车体姿态。将激光接收器安装于蒲滚上方。
设计基准作业面(作业平面高程)，作业时，激光接收器1-2与激光发射器1-3形成相对高程差。由于平地铲部件与蒲滚部件为刚性连接，故平地铲的高程变化会受蒲滚高程影响，故将激光接收器1-2安装于蒲滚上方测量基于基准作业面的相对高程hp。
如图3b所示，于蒲滚1-7上安装姿态传感器1-4以感知载体平台姿态信息ψ,φ,θ，并建立车体坐标系Oxyz；建立作业部件坐标系Ox1y1z1、Ox2y2z2，以计算液压油缸运动时作业部件末端不同程度的高程变化，如图3b平地铲运动轨迹图所示。该实施例各部分运动的相互影响描述如下：平地铲高程油缸1-5部分的伸缩将引起平地铲发生两种不同程度的高程变化；三点悬挂的升降影响蒲滚、激光接收器及后续部件的高程变化；车***姿变化引起蒲滚姿态变化继而影响所有部件的高程及姿态变化。
因此根据载体平台(蒲滚)的姿态信息ψ,
Figure BDA0002943448390000061
θ与位移传感器1-8测量的高程油缸的位移信息l1与执行机构几何关系建立计算作业部件末端基于基准作业平面的高程的解算模型
Figure BDA0002943448390000062
其中，H表示作业部件末端中心相对于基准作业平面的相对位置信息；hp表示激光接收器相对于基准作业平面的相对高程；ψ,
Figure BDA0002943448390000063
θ，分别为车体偏航角、横滚角和俯仰角；l1为执行机构上位移传感器测量值。具体地，解算步骤如下：
1)测量车体和执行机构的角度与长度信息及基于车体坐标系下激光接收器
至载体平台姿态传感器安装位置的Z轴坐标增量Zb，输入激光高程解算模型；
2)计算车体姿态旋转矩阵；
Figure BDA0002943448390000072
其中：nRb为车身姿态旋转矩阵；Ri(angle)表示绕笛卡尔(局部)坐标系i轴旋转angle角度的基本旋转矩阵，Ri T(angle)是Ri(angle)的转置矩阵。
3)根据几何关系及所测量的执行机构位置变化量，解算基于车体坐标系的作业末端中心的坐标Z轴分量rz；
rz＝f(l1)
其中，l1为执行机构上位移传感器测量值，f(x)为高程油缸伸长量与平地铲基于车体坐标系的高程变化量的关系函数。
4)计算基于车体坐标系的激光接收器至作业部件末端中心的Z轴坐标增量Ze；
Ze＝Zb+rz
5)结合步骤4)及激光传感的相对位置信息hp，计算作业部件末端中心相对于基准作业面的相对位置信息H。
hp’＝hp+E(nRb)
Figure BDA0002943448390000071
其中，E(x)为载体平台姿态变化时激光传感的补偿函数。hp’为补偿后的激光传感高程信息。hs为载体平台在水平状态下，当hp＝hp’＝0时，激光接收器与作业平面的高程差。
通过上述计算，达到应用激光高程计算作业部件末端基于基准作业平面的高程差Δh的目的。
实施例二
本发明一种基于车***姿的作业部件末端位姿测量方法，用于平地机，基于车***姿测量解算平地铲(作业部件)的位姿，如图4a所示，为装备车体2-1(轮式拖拉机)建立车体坐标系，车体坐标系Oxyz定义为以车体质心为原点O，以车体前进方向为y轴，x轴与y轴垂直指向车体方向的右侧，z轴垂直于x、y轴向上。
本实施例采用平地机，包括装备车体2-1，卫星天线2-2，液压***2-4，高程油缸2-5，水平油缸2-6，平地铲水平姿态传感器2-7，平地铲2-8(本实施例中的作业部件末端)以及姿态传感器2-3、2-9和2-10。平地铲与搭载液压***的平台以平行四边形结构连接，通过高程油缸和水平油缸分别实现平地铲的高程与水平调节。平地铲水平姿态传感器2-7安装在平地铲上，测量平地铲的水平姿态变化。姿态传感器2-9安装在介于液压***平台与平地铲间的平行四边形结构的连杆上，测量连杆的角度变化，间接反映平地铲的高程变化。液压***平台经三点悬挂挂接在车体上，将姿态传感器2-10安装于三点悬挂下拉杆处，测量下拉杆的姿态变化。姿态传感器2-3安装于上述车体坐标系的原点O处，测量车体姿态变化。将卫星天线2-2安装于拖拉机驾驶室顶部，获取车***姿信息。
获取卫星天线的位置信息、车体姿态信息和各执行机构的姿态信息，解算作业部件末端中心三维坐标，建立GNSS坐标解算模型。根据所设计基准作业面(平面或曲面)，计算作业部件末端基于基准作业面的高程差；测量作业部件的姿态信息，计算作业部件末端基于基准作业面的角度差。结合所得高程差与角度差对平地铲进行高程与调平控制，起到指导平地机精准作业的作用。
如图4a和4b所示，具体解算步骤如下：
(1)读取卫星信号接收模块信息：车体姿态角中的初始偏航角ψ，是由接收模块从天线的位置关系数据中经滤波得到。投入计算的卫星天线位置信息p是由卫星天线信息经滤波再进行坐标转换得到。
(2)姿态传感器数据采集：读取车体横滚角
Figure BDA0002943448390000081
俯仰角θ以及执行机构的姿态变化β1、β2、βe1。
(3)获取卫星天线的位置信息、车体姿态信息和各执行机构的姿态信息，建立GNSS坐标解算模型
Figure BDA0002943448390000082
计算作业部件末端中心的三维坐标。
其中，r表示作业部件末端中心的三维坐标；p表示卫星天线的位置信息；ψ,
Figure BDA0002943448390000083
θ，分别为卫星天线所测量偏航角，车体上的姿态传感器所测量横滚角和俯仰角；β1、β2为执行机构上姿态传感器测量值。具体地，解算步骤如下：
1)测量车体及执行机构的角度与长度信息和卫星天线与质心位置的坐标增量Ib，输入GNSS坐标解算模型；
2)换算车体姿态角；
从地理坐标系到车体坐标系的姿态角旋转次序为航向角ψ-俯仰角θ-横滚角
Figure BDA0002943448390000084
若使用双轴姿态传感器，则由于车体上传感器所测量的俯仰角θ与横滚角
Figure BDA0002943448390000085
与坐标系旋转计算所用并不相同，须将其中一个进行换算。由旋转次序决定，取Y轴(横滚)为坐标系变换第一个旋转轴，姿态传感器所测量横滚角
Figure BDA0002943448390000086
为姿态旋转计算所用横滚角，X轴(俯仰)为坐标系变换第二个旋转轴，此时姿态传感器所测量俯仰角θ实际是旋转后的X轴与水平面的夹角而非与原X轴夹角，因此需换算俯仰角。
根据三角关系，
Figure BDA0002943448390000091
其中，θch为换算后的俯仰角。
3)计算实际车身姿态旋转矩阵；
将车体坐标系b通过三次旋转，欧拉角旋转顺序为横滚(Y轴)-俯仰(X轴)-偏航(Z轴)，旋转到与地理坐标系n对齐，计算车身姿态旋转矩阵的公式为：
Figure BDA0002943448390000092
其中：nRb为车身姿态旋转矩阵；Ri(angle)表示绕笛卡尔(局部)坐标系i轴旋转angle角度的基本旋转矩阵，Ri T(angle)是Ri(angle)的转置矩阵。
4)根据几何关系或机器人运动学及所测量的各执行机构姿态信息及位置变化量，解算基于车体坐标系的作业部件末端中心的位置信息。
根据车体的机械结构特征选用合适的计算方式。对于简单车体机械结构可根据几何关系及各执行机构传感器测量值推算基于初始车体坐标系(车身水平状态)的作业部件末端中心的坐标ro。对于机械臂类型的工程机械/农业机械可根据机器人运动学，对于给定的机器的几何特征，已知节点变量，就能确定每根连杆的位置和方向，通过再每根连杆上建立合适坐标系，并且通过刚体运动方式来确定相邻坐标系的配置，以此解算基于初始车体坐标系(车身水平状态)的作业部件末端中心的坐标ro。实时状态下车体坐标系因地势等原因会随车体发生旋转，若执行机构上安装姿态传感器，其测量值会包括车身姿态的变化量，因此实时状态下基于车体坐标系的作业部件末端中心的坐标rr解算，须对ro进行换算。
在本实施例中选用根据几何关系及所测量的各执行机构姿态信息，解算基于车体坐标系的作业部件末端中心的坐标。
如图4b所示为平地机的计算简图，该图用于解算基于初始车体坐标系(车身水平状态)的作业部件末端中心的坐标ro：
ro＝{rox,roy,roz}
由平地机特征可知rox＝0，基于图3b，根据几何关系
roy＝-(d1+L1cosβ1+L2+L3cosβ2+d4)
roz＝-(d0-d2+L1sinβ1+L3sinβ2+d3)
其中，d0表示质心距离地面的高度，单位mm；
d1表示三点悬挂下拉杆和液压***2-4支撑架的连接点A与质心的水平距离，单位mm；
d2表示三点悬挂下拉杆和液压***2-4支撑架的连接点A距离地面的高度，单位mm；
d3表示姿态传感器2-9所在连杆D点距作业部件末端中心的垂直距离，单位mm；
d4表示姿态传感器2-9所在连杆D点与作业部件末端中心的水平距离，单位mm；
L1表示姿态传感器2-10所在连杆AB的长度，单位mm；
L2表示液压***2-4支撑架底部BC的长度，单位mm；
L3表示姿态传感器2-9所在连杆CD的长度，单位mm；
β1表示姿态传感器2-10所测姿态测量值，单位°；
β2表示姿态传感器2-9所测姿态测量值，单位°。
实时状态下车体坐标系因地势等原因会随车体发生旋转，由于姿态传感器测量值包括车身姿态的变化量，因此实时状态下基于车体坐标系的作业部件末端中心的坐标rr解算，须对机构姿态测量值进行转换。在平地机中对姿态传感器2-9和2-10有影响的是车体的俯仰角，有：
Figure BDA0002943448390000101
5)计算卫星天线与作业部件末端中心的坐标增量Ie；
Ie＝Ib+rr
6)结合步骤5)及卫星天线的位置信息，计算作业部件末端中心的三维坐标r。
r＝p+nRbIe
(4)设计基准作业面(平面或曲面)即设计作业面角度α及高程h，实时测量平地铲水平姿态βe1，计算作业部件末端机具基于基准作业面的角度差Δα：Δα＝βe1-α；基于所求作业部件末端的三维坐标的Z轴分量rz，计算作业部件末端与基准作业面的高程差Δh。
实施例三
本发明一种基于车***姿的作业部件末端位姿测量方法，用于四自由度机械臂的挖掘机，基于车***姿测量解算铲斗(作业部件末端)的位姿。该实施例所建立的GNSS坐标解算模型如下：
Figure BDA0002943448390000102
其中，r表示作业部件末端中心的三维坐标；p表示卫星天线的位置信息；ψ,
Figure BDA0002943448390000111
θ，分别为车体偏航角、横滚角和俯仰角；β1,β2,β3,β4为各执行机构上姿态传感器测量值；
解算的其他步骤与实施例二相似，唯有根据解算基于车体坐标系的作业部件末端中心的位置信息部分可有不同，可如实施例二般利用几何关系推算作业部件末端中心的坐标值，也可根据机器人运动学，对于给定的机器的几何特征，已知节点变量，就能确定每根连杆的位置和方向，通过再每根连杆上建立合适坐标系，并且通过刚体运动方式来确定相邻坐标系的配置，以此解算基于初始车体坐标系(车身水平状态)的作业部件末端中心的坐标ro。
关节空间到位姿空间的转换为正运动学问题，建立挖掘机器人D-H坐标系，从基坐标系到铲斗坐标系的变换0T4为：0T4＝0T1 1T2 2T3 3T4
其中，i-1Ti是基于D-H法的规定，将坐标系Bi变换至坐标系Bi-1的变换矩阵，可以用连杆(i)和关节i的4个基本变换的乘积表达。
而铲斗末端在基坐标系中的位置为：
Figure BDA0002943448390000112
在实时状态下车体坐标系因地势等原因会随车体发生旋转，由于倾角传感器测量值包括车身姿态的变化量，因此实时状态下基于车体坐标系的作业部件末端中心的坐标rr解算，须对ro进行换算：
rr＝RX(-θ)ro
其中，Ri(angle)表示绕笛卡尔(局部)坐标系i轴旋转angle角度的基本旋转矩阵。
由于实施例所示机械运动轨迹较为复杂，同时部分机械的作业部件末端(例如挖掘机的铲斗)由于工作方式等问题不适合直接在末端安装GNSS天线或激光传感来直接感知末端的位置信息。考虑到工作环境的安全性、装配难度及运动过程易损坏设备，故而应用基于车***姿测量的作业部件末端位姿测量方法。同时复杂造型施工和水下作业等要求测量快速获得高精度的作业部件末端的位置信息，而GNSS***或激光传感难以直接测量获得的情况也可应用本发明提出的测量方法。
上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (10)

1.一种基于车***姿的作业部件末端位姿测量方法，其特征在于：包括如下步骤：
a、将姿态传感器安装于车体或与车体刚性连接的载体平台上，测量车体姿态；
b、将GNSS***中的卫星天线安装于车体顶部，测量车体的位姿信息；或将激光传感安装于车体或载体平台上，测量基于基准作业面的相对位置信息；
c、建立车体坐标系Oxyz，以车体前进方向为y轴，x轴与y轴垂直指向车体方向的右侧，z轴垂直于x、y轴向上；
d、在各个执行机构安装传感器，检测执行机构的姿态变化以及位置变化，并建立作业部件坐标系Oxnynzn，n＝1,2...；
e、获取卫星天线或激光传感的位置信息，获取车体姿态信息，获取执行机构的姿态信息及位置变化量，建立基于所获取信息及执行机构几何关系求解作业部件末端中心的位置解算模型，并计算获得作业部件末端中心坐标；
f、测量作业部件的姿态信息并推算作业部件末端的姿态信息；
g、设计基准作业面，结合所求的作业部件末端位姿信息计算作业部件末端与基准作业面的高程差Δh，以及作业部件末端与基准作业面的角度差Δα。
2.按照权利要求1所述的一种基于车***姿的作业部件末端位姿测量方法，其特征在于：步骤b中，GNSS***用于计算偏航角ψ及空间位置坐标。
3.按照权利要求1所述的一种基于车***姿的作业部件末端位姿测量方法，其特征在于：步骤d中，根据执行机构的运动需求，传感器选择姿态传感器或位移传感器。
4.按照权利要求1所述的一种基于车***姿的作业部件末端位姿测量方法，其特征在于：步骤e中，根据卫星天线的位置信息、车体姿态信息和各执行机构的姿态信息及位置变化量，建立GNSS坐标解算模型，解算作业部件末端三维坐标，GNSS坐标解算模型如下：
Figure FDA0002943448380000011
其中，n＝1,2,...；r表示作业部件末端中心的三维坐标；p表示卫星天线的位置信息；ψ,
Figure FDA0002943448380000012
θ分别为车体偏航角、横滚角和俯仰角；β1,β2,......,βn为各执行机构上姿态传感器测量值；l1,l2,......,ln为各执行机构上位移传感器测量值。
5.按照权利要求4所述的一种基于车***姿的作业部件末端位姿测量方法，其特征在于：解算作业部件末端三维坐标步骤如下：
(1)测量车体及执行机构的角度与长度信息和卫星天线至车体质心位置的坐标增量，输入GNSS坐标解算模型；
(2)换算车体姿态角；
(3)计算车身姿态旋转矩阵；
(4)根据几何关系或机器人运动学及所测量的各执行机构姿态信息与位置变化量，解算基于车体坐标系的作业部件末端中心的位置信息；
(5)计算卫星天线至作业部件末端中心的坐标增量；
(6)结合步骤(5)及卫星天线的位置信息，计算作业部件末端中心的三维坐标。
6.按照权利要求1所述的一种基于车***姿的作业部件末端位姿测量方法，其特征在于：步骤e中，根据激光传感的位置信息、车体姿态信息和各执行机构的姿态信息及位置变化量，建立激光高程解算模型，求解作业部件末端中心的高程信息，激光高程解算模型如下：
Figure FDA0002943448380000021
其中，n＝1,2,...；H表示作业部件末端中心相对于基准作业面的相对位置信息；hp表示激光接收器相对于基准作业面的相对高程；ψ,
Figure FDA0002943448380000022
θ分别为车体偏航角、横滚角和俯仰角；β1,β2,......,βn为各执行机构上姿态传感器测量值；l1,l2,......,ln为各执行机构上位移传感器测量值。
7.按照权利要求6所述的一种基于车***姿的作业部件末端位姿测量方法，其特征在于：作业部件末端中心高程解算步骤如下：
(1)测量车体及执行机构的角度与长度信息和激光接收器至车体或载体平台姿态传感器安装位置的Z轴坐标增量，输入激光高程解算模型；
(2)计算车体姿态旋转矩阵；
(3)根据几何关系或机器人运动学及所测量的各执行机构姿态信息与位置变化量，解算基于车体坐标系的作业部件末端中心的坐标Z轴分量；
(4)计算激光接收器至作业部件末端中心的Z轴坐标增量；
(5)结合步骤(4)及激光传感的位置信息，计算作业部件末端中心相对于基准作业面的相对位置信息。
8.按照权利要求1所述的一种基于车***姿的作业部件末端位姿测量方法，其特征在于：步骤g中，设计基准作业面包括高程h，基于所求作业部件末端的三维坐标的Z轴分量，计算作业部件末端在基准作业面各位置处的高程差；在激光传感应用上，基准作业面为平面，激光接收器与激光发射器形成相对高程差，在设计基准作业面的同时，可知激光接收器与基准作业面的高程差。
9.按照权利要求1所述的一种基于车***姿的作业部件末端位姿测量方法，其特征在于：步骤g中，设计基准作业面包括设计基准作业面角度α，取与基准作业面相关的作业部件末端姿态，计算作业部件末端基于基准作业面的角度差。
10.按照权利要求4所述的一种基于车***姿的作业部件末端位姿测量方法，其特征在于：车体姿态角中的初始偏航角ψ，是由卫星信号接收模块从卫星天线的位置关系数据中经滤波得到或与姿态传感器所测偏航角融合得到；用于计算的卫星天线位置信息由天线初始信息经滤波再进行坐标转换得到。
CN202110188180.0A 2021-02-18 2021-02-18 一种基于车***姿的作业部件末端位姿测量方法 Active CN112987069B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110188180.0A CN112987069B (zh) 2021-02-18 2021-02-18 一种基于车***姿的作业部件末端位姿测量方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110188180.0A CN112987069B (zh) 2021-02-18 2021-02-18 一种基于车***姿的作业部件末端位姿测量方法

Publications (2)

Publication Number Publication Date
CN112987069A true CN112987069A (zh) 2021-06-18
CN112987069B CN112987069B (zh) 2021-11-26

Family

ID=76393731

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110188180.0A Active CN112987069B (zh) 2021-02-18 2021-02-18 一种基于车***姿的作业部件末端位姿测量方法

Country Status (1)

Country Link
CN (1) CN112987069B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113405554A (zh) * 2021-06-29 2021-09-17 东风汽车集团股份有限公司 基于卫星定位***的车辆姿态计算方法
CN114187353A (zh) * 2021-10-18 2022-03-15 北京理工大学 基于视觉的智能稻麦收割机拨禾轮位置测量方法
CN117040608A (zh) * 2023-10-10 2023-11-10 四川轻化工大学 一种车载卫星中继

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2009035603A2 (en) * 2007-09-12 2009-03-19 Topcon Positioning Systems, Inc. Automatic blade control system with integrated global navigation satellite system and inertial sensors
CN102607499A (zh) * 2012-03-12 2012-07-25 华南农业大学 一种车载农田三维地形测量装置
CN105606049A (zh) * 2015-09-11 2016-05-25 华南农业大学 一种相对水面的水田农业机具作业姿态测量方法

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108132476B (zh) * 2017-12-29 2019-03-01 北京理工大学 一种工程机械料斗定位***及方法
CN110608714A (zh) * 2019-08-01 2019-12-24 江苏科博空间信息科技有限公司 一种基于北斗/gnss塔吊姿态自动监测方法
CN111044073B (zh) * 2019-11-26 2022-07-05 北京卫星制造厂有限公司 基于双目激光高精度agv位置感知方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2009035603A2 (en) * 2007-09-12 2009-03-19 Topcon Positioning Systems, Inc. Automatic blade control system with integrated global navigation satellite system and inertial sensors
CN102607499A (zh) * 2012-03-12 2012-07-25 华南农业大学 一种车载农田三维地形测量装置
CN105606049A (zh) * 2015-09-11 2016-05-25 华南农业大学 一种相对水面的水田农业机具作业姿态测量方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
RUNMAO ZHAO ET AL.: "Method of estimating vertical kinematic states of working implements based on laser receivers and accelerometers", 《BIOSYSTEMS ENGINEERING》 *
刘寅 等: "基于GPS控制技术的土地平整***", 《农机化研究》 *
陈嘉琪 等: "水田激光平地机调平***动力学建模", 《农业工程学报》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113405554A (zh) * 2021-06-29 2021-09-17 东风汽车集团股份有限公司 基于卫星定位***的车辆姿态计算方法
CN114187353A (zh) * 2021-10-18 2022-03-15 北京理工大学 基于视觉的智能稻麦收割机拨禾轮位置测量方法
CN117040608A (zh) * 2023-10-10 2023-11-10 四川轻化工大学 一种车载卫星中继
CN117040608B (zh) * 2023-10-10 2023-12-08 四川轻化工大学 一种车载卫星中继

Also Published As

Publication number Publication date
CN112987069B (zh) 2021-11-26

Similar Documents

Publication Publication Date Title
CN112987069B (zh) 一种基于车***姿的作业部件末端位姿测量方法
CN109614743B (zh) 挖掘机及其铲斗定位方法、电子设备、存储介质
CN108350679B (zh) 机动平地机的铲刀自动控制***
JP5873607B1 (ja) 作業機械の較正装置、及び作業機械の作業機パラメータの較正方法
CN110392754B (zh) 工程机械
US9157216B2 (en) Hydraulic shovel calibration device and hydraulic shovel calibration method
US8886416B2 (en) Hydraulic shovel operability range display device and method for controlling same
CN101266134B (zh) 悬臂掘进机头位姿的测量***及其方法
CN103080426B (zh) 液压挖掘机的校准***及校准方法
US8352131B2 (en) Drainage plow control and method of controlling an elevation of a cutting edge of a drainage plow
KR101739309B1 (ko) 작업 기계, 및 작업 기계의 작업기 파라미터 보정 방법
KR101495582B1 (ko) 유압 셔블의 교정 장치 및 유압 셔블의 교정 방법
JP5841300B1 (ja) 作業機械の較正装置、及び作業機械の作業機パラメータの較正方法
US20080087447A1 (en) Control and method of control for an earthmoving system
CN113494104B (zh) 建设作业机械的定位校正方法及其定位校正控制器
JPH10183675A (ja) モービル機械に取付けられ、該モービル機械に対し可動な作業具上の点の位置を求めるための装置と方法
CN111485879B (zh) 一种掘进机车体及其截割滚筒的定位方法及定位***
CN109070353B (zh) 尤其是混凝土泵的大型操纵器的臂架末端的笛卡尔控制
WO2023072044A1 (zh) 挖掘机铲斗齿尖定位方法、装置和挖掘机
CN201247045Y (zh) 悬臂掘进机头位姿的测量***
JP2017211265A (ja) 測量装置、移動体、測量方法及びプログラム
WO2021253958A1 (zh) 一种基于卫星导航的挖掘机智能化高精度定位方法
JP7424960B2 (ja) 情報取得システムおよび情報取得方法
CN112817019A (zh) 一种基于姿态信息的可控震源车工作点定位误差补偿方法
CN111441406B (zh) 用于坡度控制的鸟瞰校准

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
GR01 Patent grant
GR01 Patent grant