CN115046484A - 高度获取装置、方法及计算机可读存储介质 - Google Patents

Abstract

本申请公开了一种高度获取装置、方法及计算机可读存储介质，涉及测绘技术领域。本申请包括：所述高度获取装置用于获取目标物件的高度，所述高度获取装置包括：位置获取模块用于获取目标物件的顶点在雷达坐标系下的第一坐标；距离获取模块用于根据所述第一坐标获取所述目标物件的顶点与所述雷达坐标系的原点之间的距离；第一高度获取模块，所述第一高度获取模块连接所述位置获取模块，所述第一高度获取模块用于获取所述激光雷达的离地高度；姿态角获取模块，所述姿态角获取模块用于获取所述激光雷达的姿态角；第二高度获取模块用于获取所述目标物件的高度。本申请能够应用激光雷达获取物件的离地高度，在进行离地高度计算时计算量低。

Description

高度获取装置、方法及计算机可读存储介质

技术领域

本申请涉及测绘技术领域，具体涉及一种高度获取装置、方法及计算机可读存储介质。

背景技术

激光雷达(Lidar)按照扫描线束的不同可以分为单线激光雷达与多线激光雷达。单线激光雷达即激光源垂直方向发出的扫描线束为单线的雷达。单线激光雷达由于具有扫描速度快、分辨率强、可靠性高等优势，得到了广泛的使用。然而，单线激光雷达由于只有一条扫描线束，因此通常用于平面建图，难以实现对地面物体高度的测量。

发明内容

鉴于此，本申请提供一种高度获取装置、方法及计算机可读存储介质，能够通过激光雷达的离地高度及激光雷达的姿态角获取目标物件顶点的离地高度，可以适用于单线激光雷达和多线激光雷达，在应用于单线激光雷达时可以使单线激光雷达能够测量离地高度，当应用于多线激光雷达时，可以仅通过一条扫描线束获取目标物件的高度，可以减少高度获取时的计算量。

本申请第一方面提供一种高度获取装置，应用于激光雷达，所述高度获取装置用于获取目标物件的高度，所述高度获取装置包括：位置获取模块，所述位置获取模块用于获取所述目标物件的顶点在雷达坐标系下的第一坐标；距离获取模块，所述距离获取模块连接所述位置获取模块，所述距离获取模块用于根据所述第一坐标获取所述目标物件的顶点与所述雷达坐标系的原点之间的距离；第一高度获取模块，所述第一高度获取模块连接所述位置获取模块，所述第一高度获取模块用于获取所述激光雷达的离地高度；姿态角获取模块，所述姿态角获取模块用于获取所述激光雷达的姿态角；第二高度获取模块，所述第二高度获取模块连接所述距离获取模块、所述第一高度获取模块及所述姿态角获取模块，所述第二高度获取模块用于根据所述目标物件的顶点与所述雷达坐标系的原点之间的距离、所述激光雷达的离地高度、所述激光雷达的姿态角获取所述目标物件的高度。

根据本申请一种可能的实施方式，所述激光雷达的姿态角包括所述激光雷达的第一姿态角及第二姿态角；所述第二高度获取模块用于根据以下公式获取所述目标物件的高度H2：H2＝H1-D*cos(spin)*sin(pitch)；其中，H1为所述激光雷达的离地高度，D为所述目标物件的顶点与所述雷达坐标系的原点之间的距离，pitch为所述第一姿态角，spin为所述第二姿态角。

根据本申请一种可能的实施方式，所述激光雷达包括单线激光雷达或者多线激光雷达。

根据本申请一种可能的实施方式，当所述激光雷达为所述单线激光雷达时，所述姿态角获取模块还用于：对所述单线激光雷达进行标定以获取所述第一姿态角；其中，所述对所述单线激光雷达进行标定以获取所述第一姿态角，包括：获取所述单线激光雷达的扫描线与地面的交线；获取所述交线上距离所述雷达坐标系的原点最近的位置点；获取所述位置点在所述雷达坐标系下的位置点坐标；根据所述位置点坐标获取所述位置点与所述雷达坐标系的原点之间的距离；根据所述单线激光雷达的离地高度、所述位置点与所述雷达坐标系的原点之间的距离获取所述第一姿态角。

根据本申请一种可能的实施方式，当所述激光雷达为所述多线激光雷达时，所述多线激光雷达用于应用第一线束扫描所述目标物件，所述第一线束为所述多线激光雷达中具有预设纬度角的扫描线束；所述姿态角获取模块还用于：对所述多线激光雷达进行标定以获取所述第一姿态角；其中，所述对所述多线激光雷达进行标定以获取所述第一姿态角，包括：获取所述第一线束与地面的交线；获取所述第一线束的纬度角；获取所述交线上距离所述雷达坐标系的原点最近的位置点；获取所述位置点在所述雷达坐标系下的位置点坐标；根据所述位置点坐标获取所述位置点与所述雷达坐标系的原点之间的距离；根据所述多线激光雷达的离地高度、所述位置点与所述雷达坐标系的原点之间的距离以及所述第一线束的纬度角获取所述第一姿态角。

根据本申请一种可能的实施方式，所述姿态角获取模块还用于：对所述激光雷达进行标定以获取所述第二姿态角；其中，所述对所述激光雷达进行标定以获取所述第二姿态角，包括：根据所述位置点在所述雷达坐标系下的所述位置点坐标以及所述雷达坐标系的原点坐标确定第一向量；获取所述第一向量与所述雷达坐标系的X轴之间的第一夹角；所述第一夹角为所述第二姿态角。

根据本申请一种可能的实施方式，所述激光雷达的姿态角还包括所述激光雷达的第三姿态角：对所述激光雷达进行标定以获取所述第三姿态角；其中，所述对所述激光雷达进行标定以获取所述第三姿态角，包括：响应于标定物和所述激光雷达发生相对移动，通过所述激光雷达扫描所述标定物，获取发生相对移动前所述标定物的标定点在所述雷达坐标系下的第一标定坐标以及发生相对移动后标定物的标定点在所述雷达坐标系下的第二标定坐标；根据所述第一标定坐标以及所述第二标定坐标确定第二向量；获取所述第一向量和所述第二向量之间的第二夹角；根据所述第二夹角以及所述第一姿态角获取所述第三姿态角。

根据本申请一种可能的实施方式，所述标定物包括直角棱柱或三角棱柱；扫描所述直角棱柱或所述三角棱柱以获取回波角点，所述回波角点为所述标定点。

本申请第二方面提供一种高度获取方法，应用于激光雷达，所述高度获取方法包括：获取目标物件的顶点在雷达坐标系下的第一坐标；根据所述第一坐标获取所述目标物件的顶点与所述雷达坐标系的原点之间的距离；获取所述激光雷达的离地高度；获取所述激光雷达的姿态角；根据所述目标物件的顶点与所述雷达坐标系的原点之间的距离、所述激光雷达的离地高度、所述激光雷达的姿态角获取所述目标物件的高度。

本申请第三方面提供一种计算机可读存储介质，所述计算机存储介质存储有计算机程序，当所述计算机程序被处理器执行时，使所述处理器执行如第二方面所述的高度获取方法。

本申请通过将激光雷达安装在一预设的离地高度，能够通过激光雷达的离地高度及激光雷达的姿态角获取目标物件顶点的离地高度，可以适用于单线激光雷达和多线激光雷达，在应用于单线激光雷达时可以使单线激光雷达能够测量离地高度，当应用于多线激光雷达时，可以仅通过一条扫描线束获取目标物件的高度，可以减少高度获取时的计算量。

附图说明

图1是本申请一个实施方式提供的多足机器人的硬件结构示意图。

图2是本申请另一个实施方式提供的多足机器人的硬件结构示意图。

图3是本申请一个实施方式提供的高度获取装置的模块示意图。

图4是本申请一个实施方式提供的标定物获取示意图。

图5是本申请一个实施方式提供的激光雷达的扫描透视示意图。

图6是本申请另一个实施方式提供的激光雷达的扫描透视示意图。

图7是本申请另一个实施方式提供的激光雷达的扫描透视示意图。

图8是本申请一个实施方式提供的激光雷达的高度获取的示意图。

图9是本申请一实施方式提供的高度获取示意图。

图10是本申请一个实施方式提供的高度获取方法的流程示意图。

图11是本申请另一个实施方式提供的高度获取方法的流程示意图。

主要元件符号说明

多足机器人 100

机械单元 101

驱动板 1011

电动机 1012

机械结构 1013

机身主体 1014

腿部 1015

足部 1016

头部结构 1017

尾巴结构 1018

载物结构 1019

鞍座结构 1020

摄像头结构 1021

通讯单元 102

传感单元 103

接口单元 104

存储单元 105

显示单元 106

显示面板 1061

输入单元 107

触控面板 1071

其他输入设备 1072

触摸检测装置 1073

触摸控制器 1074

控制模块 110

电源 111

高度获取装置 200

位置获取模块 210

距离获取模块 220

第一高度获取模块 230

姿态角获取模块 240

第二高度获取模块 250

激光雷达 300

回波角点 N1

具体实施方式

需要说明的是，本申请实施例中“至少一个”是指一个或者多个，“多个”是指两个或多于两个。“和/或”，描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B可以表示：单独存在A，同时存在A和B，单独存在B的情况，其中A，B可以是单数或者复数。本申请的说明书和权利要求书及附图中的术语“第一”、“第二”、“第三”、“第四”等(如果存在)是用于区别类似的对象，而不是用于描述特定的顺序或先后次序。

另外需要说明的是，本申请实施例中公开的方法或流程图所示出的方法，包括用于实现方法的一个或多个步骤，在不脱离权利要求的范围的情况下，多个步骤的执行顺序可以彼此互换，其中某些步骤也可以被删除。

激光雷达(Lidar)按照扫描线束的不同可以分为单线激光雷达与多线激光雷达。单线激光雷达即激光源垂直方向发出的扫描线束为单线的雷达。单线激光雷达由于具有扫描速度快、分辨率强、可靠性高等优势，得到了广泛的使用。然而，单线激光雷达由于只有一条扫描线束，因此通常用于平面建图，难以实现对地面物体高度的测量。

因此，本申请实施例提供一种高度获取装置，能够应用单线激光雷达获取目标物件的离地高度，或者通过多线激光雷达中的一条扫描线束获取目标物件的离地高度。

请参阅图1，图1为本申请其中一个实施方式的多足机器人100的硬件结构示意图。在图1所示的实施方式中，多足机器人100包括机械单元101、通讯单元102、传感单元103、接口单元104、存储单元105、控制模块110、电源111。多足机器人100的各种部件可以以任何方式连接，包括有线或无线连接等。本领域技术人员可以理解，图1中示出的多足机器人100的具体结构并不构成对多足机器人100的限定，多足机器人100可以包括比图示更多或更少的部件，某些部件也并不属于多足机器人100的必须构成，完全可以根据需要在不改变发明的本质的范围内而省略，或者组合某些部件。

下面结合图1对多足机器人100的各个部件进行具体的介绍：

机械单元101为多足机器人100的硬件。如图1所示，机械单元101可包括驱动板1011、电动机1012、机械结构1013，如图2所示，机械结构1013可包括机身主体1014、可伸展的腿部1015、足部1016，在其他实施方式中，机械结构1013还可包括可伸展的机械臂(图未示)、可转动的头部结构1017、可摇动的尾巴结构1018、载物结构1019、鞍座结构1020、摄像头结构1021等。需要说明的是，机械单元101的各个部件模块可以为一个也可以为多个，可根据具体情况设置，比如腿部1015可为4个，每个腿部1015可配置3个电动机1012，对应的电动机1012为12个。

通讯单元102可用于信号的接收和发送，还可以通过与网络和其他设备通信，比如，接收遥控器或其他多足机器人100发送的按照特定步态以特定速度值向特定方向移动的指令信息后，传输给控制模块110处理。通讯单元102包括如WiFi模块、4G模块、5G模块、蓝牙模块、红外模块等。

传感单元103用于获取多足机器人100周围环境的信息数据以及监控多足机器人100内部各部件的参数数据，并发送给控制模块110。传感单元103包括多种传感器，如获取周围环境信息的传感器：激光雷达(用于远程物体检测、距离确定和/或速度值确定)、毫米波雷达(用于短程物体检测、距离确定和/或速度值确定)、摄像头、红外摄像头、全球导航卫星系统(GNSS，Global Navigation Satellite System)等。如监控多足机器人100内部各部件的传感器：惯性测量单元(IMU，Inertial Measurement Unit)(用于测量速度值、加速度值和角速度值的值)，足底传感器(用于监测足底着力点位置、足底姿态、触地力大小和方向)、温度传感器(用于检测部件温度)。至于多足机器人100还可配置的载荷传感器、触摸传感器、电机角度传感器、扭矩传感器等其他传感器，在此不再赘述。

接口单元104可以用于接收来自外部装置的输入(例如，数据信息、电力等)并且将接收到的输入传输到多足机器人100内的一个或多个部件，或者可以用于向外部装置输出(例如，数据信息、电力等)。接口单元104可包括电源端口、数据端口(如USB端口)、存储卡端口、用于连接具有识别模块的装置的端口、音频输入/输出(I/O)端口、视频I/O端口等。

存储单元105用于存储软件程序以及各种数据。存储单元105可主要包括程序存储区和数据存储区，其中，程序存储区可存储操作系统程序、运动控制程序、应用程序(比如文本编辑器)等；数据存储区可存储多足机器人100在使用中所生成的数据(比如传感单元103获取的各种传感数据，日志文件数据)等。此外，存储单元105可以包括高速随机存取存储器，还可以包括非易失性存储器，例如磁盘存储器、闪存器、或其他易失性固态存储器。

显示单元106用于显示由用户输入的信息或提供给用户的信息。显示单元106可包括显示面板1061，可以采用液晶显示器(Liquid Crystal Display，LCD)、有机发光二极管(Organic Light-Emitting Diode,OLED)等形式来配置显示面板1061。

输入单元107可用于接收输入的数字或字符信息。具体地，输入单元107可包括触控面板1071以及其他输入设备1072。触控面板1071，也称为触摸屏，可收集用户的触摸操作(比如用户使用手掌、手指或适合的附件在触控面板1071上或在触控面板1071附近的操作)，并根据预先设定的程式驱动相应的连接装置。触控面板1071可包括触摸检测装置1073和触摸控制器1074两个部分。其中，触摸检测装置1073检测用户的触摸方位，并检测触摸操作带来的信号，将信号传送给触摸控制器1074；触摸控制器1074从触摸检测装置1073上接收触摸信息，并将它转换成触点坐标，再送给控制模块110，并能接收控制模块110发来的命令并加以执行。除了触控面板1071，输入单元107还可以包括其他输入设备1072。具体地，其他输入设备1072可以包括但不限于遥控操作手柄等中的一种或多种，具体此处不做限定。

进一步的，触控面板1071可覆盖显示面板1061，当触控面板1071检测到在其上或附近的触摸操作后，传送给控制模块110以确定触摸事件的类型，随后控制模块110根据触摸事件的类型在显示面板1061上提供相应的视觉输出。虽然在图1中，触控面板1071与显示面板1061是作为两个独立的部件来分别实现输入和输出功能，但是在某些实施例中，可以将触控面板1071与显示面板1061集成而实现输入和输出功能，具体此处不做限定。

控制模块110是多足机器人100的控制中心，利用各种接口和线路连接整个多足机器人100的各个部件，通过运行或执行存储在存储单元105内的软件程序，以及调用存储在存储单元105内的数据，从而对多足机器人100进行整体控制。

电源111用于给各个部件供电，电源111可包括电池和电源控制板，电源控制板用于控制电池充电、放电、以及功耗管理等功能。在图1所示的实施方式中，电源111电连接控制模块110，在其它的实施方式中，电源111还可以分别与传感单元103(比如摄像头、雷达、音箱等)、电动机1012电性连接。需要说明的是，各个部件可以各自连接到不同的电源111，或者由相同的电源111供电。

在上述实施方式的基础上，具体地，在一些实施方式中，可以通过终端设备来与多足机器人100进行通信连接，在终端设备与多足机器人100进行通信时，可以通过终端设备来向多足机器人100发送指令信息，多足机器人100可通过通讯单元102来接收指令信息，并可在接收到指令信息的情况下，将指令信息传输至控制模块110，使得控制模块110可根据指令信息来处理得到目标速度值。终端设备包括但不限于：具备图像拍摄功能的手机、平板电脑、服务器、个人计算机、可穿戴智能设备、其它电器设备。

指令信息可以根据预设条件来确定。在一个实施方式中，多足机器人100可以包括传感单元103，传感单元103可根据多足机器人100所在的当前环境可生成指令信息。控制模块110可根据指令信息来判断多足机器人100的当前速度值是否满足对应的预设条件。若满足，则会保持多足机器人100的当前速度值和当前步态移动；若不满足，则会根据对应的预设条件来确定目标速度值和相应的目标步态，从而可控制多足机器人100以目标速度值和相应的目标步态移动。环境传感器可以包括温度传感器、气压传感器、视觉传感器、声音传感器。指令信息可以包括温度信息、气压信息、图像信息、声音信息。环境传感器与控制模块110之间的通信方式可以为有线通信，也可以为无线通信。无线通信的方式包括但不限于：无线网络、移动通信网络(3G、4G、5G等)、蓝牙、红外。

请一并参阅图3，图3是本申请一实施例提供的高度获取装置200。如图3所示，高度获取装置200包括位置获取模块210、距离获取模块220、第一高度获取模块230、姿态角获取模块240及第二高度获取模块250，高度获取装置200连接至激光雷达300。可以理解，激光雷达300可以是单线激光雷达，也可以是多线激光雷达。

在本实施例中，高度获取装置200可以作为独立的装置安装在多足机器人100上。高度获取装置200也可以是多足机器人100的一部分，例如，使用多足机器人100上的控制模块110作为位置获取模块210、距离获取模块220、第一高度获取模块230、姿态角获取模块240及第二高度获取模块250，并使用传感单元103中的激光雷达作为激光雷达300。

在本实施例中，以激光雷达300为单线激光雷达为例，描述高度获取装置200的工作过程。可以理解，激光雷达300安装在一预设的离地高度上，并朝向地面倾斜一定的角度，例如设置在多足机器人100的头部结构1017、机身主体1014等位置。在安装完成后，测量目标物件的高度时不需再调整激光雷达300的离地高度。例如，离地高度可以设置为20cm、30cm或40cm等，本申请对此不做限制。可以理解，在激光雷达300安装完成后，将激光雷达300的雷达坐标系的原点到地面距离存储在存储单元105中，以供第一高度获取模块230获取激光雷达300的高度。

可以理解，若要调整激光雷达300的安装位置，则需要在激光雷达300的安装位置调整结束后，将调整后的激光雷达300的雷达坐标系的原点到地面距离存储在存储单元105中。

在本实施例中，高度获取装置200在获取目标物件的高度之前，需要先寻找标定物，并通过姿态角获取模块240根据标定物获取激光雷达300的姿态角，以根据激光雷达300的姿态角获取目标物件的高度。

在本实施例中，标定物可以是直角棱柱或三角棱柱等，本申请对此不做限制。可以理解，在激光雷达300扫描直角棱柱或三角棱柱上的角点时，会在接收到的激光扫描图像中收到一个激光回波角点，该激光回波角点对应与直角棱柱或三角棱柱上的角点，因此姿态角获取模块240可以通过激光雷达300扫描时获取的激光回波角点的坐标来标定激光雷达300的姿态角。

例如，请一并参阅图4，图4是本申请一个实施方式提供的标定物获取示意图。高度获取装置200可以控制激光雷达300开启扫描，并在获取到回波角点后，开始对激光雷达300的姿态角进行标定。

在本实施例中，激光雷达300在扫描标定物时，由于标定物是直角棱柱或三角棱柱，因此标定物的棱角会产生回波角点N1。在姿态角获取模块240可以根据回波角点N1对激光雷达300进行标定，以获取激光雷达300的姿态角。

请一并参阅图5，图5是本申请一个实施方式提供的激光雷达300的扫描透视示意图。

可以理解，激光雷达300的姿态角包括第一姿态角pitch。第一姿态角pitch即激光雷达300安装时的俯仰角。激光雷达300在扫描时，会以自身上的某一点为原点建立一个雷达坐标系。其中，雷达坐标系为右手坐标系，包括x轴、y轴及z轴，x轴对应于激光雷达300的朝向，y轴向激光雷达300的朝向的左侧，z轴朝上。由于激光雷达300安装在预设的离地高度，且激光雷达的扫描方向朝向地面倾斜，因此激光雷达300的扫描线与地面的交线OO′为一条直线。获取装置200获取交线OO′上与雷达坐标系的原点L距离最近的位置点A。可以理解，位置点A在雷达坐标系中的坐标(x1，y1，z1)即为雷达坐标系下的位置点坐标。

可以理解，雷达坐标系的原点L与位置点A之间的连线LA垂直于交线OO′。因此，姿态角获取模块240可以通过公式(1)获取激光雷达300的第一姿态角。

其中，pitch为激光雷达300的第一姿态角，H1为激光雷达300的雷达坐标系的原点离地高度，LA为位置点A距离雷达坐标系的原点L的距离。

请一并参阅图6，图6是本申请另一个实施方式提供的激光雷达300的扫描透视示意图。

可以理解，激光雷达300的姿态角还包括第三姿态角yaw。激光雷达300会以自身的某一点为原点L建立一个雷达坐标系，G为激光雷达300的原点L在地面的正投影点。例如，雷达坐标系的原点L可以是激光雷达300的中心点。

连接激光雷达300的坐标系原点L与激光雷达300的坐标系原点L在地面的正投影点G，可以获得直线LG。此时，由于LG是垂直于地面的，而直线OO′在地面上，因此直线LG垂直于直线OO′。由于连线LA是雷达坐标系的原点L与交线OO′上与雷达坐标系的原点L距离最近的位置点A的连线，因此LA也垂直于交线OO′，因此交线OO′垂直于平面LGA。

连接激光雷达300的坐标系原点L在地面的正投影点G与位置点A，可以获得连线GA。由于连线GA位于平面LGA上，且平面LGA垂直于交线OO′，因此连线GA也垂直于交线OO′。

可以理解，在本实施例中，可以保持标定物(例如直角棱柱或三角棱柱)竖直朝上，高度获取装置200平行于地面移动，将地面视为水平面，则高度获取装置200在水平面上移动。分别获取高度获取装置200在移动前标定物的回波角点在雷达坐标系内的第一标定点F1及第一标定点F1的坐标值第一标定坐标，以及高度获取装置200在移动后的回波角点在雷达坐标系内的第二标定点F2及第二标定点F2的坐标值第二标定坐标。

在另一些实施例中，还可以保持高度获取装置200的位置不变，移动标定物(例如直角棱柱或三角棱柱)，以获取标定物的回波角点在雷达坐标系内的第一标定点F1及第二标定点F2。即使得标定物和高度获取装置200(激光雷达300)发生相对移动。

在本实施例中，如图6所示，以保持标定物(例如直角棱柱或三角棱柱)的位置不变，高度获取装置200沿与行进方向平行的方向移动为例，连接第一标定点F1与第二标定点F2可以获取向量

连接激光雷达300的雷达坐标系的原点L与位置点A可以获取向量

向量

与向量

之间的夹角为α。

请一并参阅图7，图7是本申请另一个实施方式提供的激光雷达300的扫描透视示意图。

可以理解，以激光雷达300的雷达坐标系的原点L为原点，沿着平行向量

的方向做一条射线，射线会与交线OO′相交于点B。平面ALB即雷达坐标系的X轴与Y轴组成的平面。

在本实施例中，激光雷达300距离地面的高度为LG，LG＝H1。∠LAG为激光雷达300的第一姿态角pitch，可以根据公式(2)至(4)获取连线LA、连线AB以及连线AG的长度。

其中，H1为激光雷达300距离地面的高度，α为向量

与向量

之间的夹角，pitch为激光雷达300的第一姿态角。

可以理解，因为连线AG垂直于连线AB，可以得出第三姿态角yaw，yaw＝∠BGA，因此可以根据公式(5)获取激光雷达300的第三姿态角yaw。

可以理解，激光雷达300的姿态角还包括第二姿态角spin。激光雷达300的第二姿态角spin为连线LA与雷达坐标系的x轴之间的第一夹角。即第二姿态角spin为姿态角获取模块240根据位置点A在雷达坐标系下的位置点坐标(x1，y1，z1)以及雷达坐标系的原点L的坐标确定第一向量

姿态角获取模块240获取第一向量

与雷达坐标系的X轴之间的第一夹角。

可以理解，第二姿态角spin为雷达坐标系围绕z轴旋转两次获得的自旋角。

可以理解，根据激光雷达300的第三姿态角yaw、激光雷达300的第一姿态角pitch以及激光雷达300的第二姿态角spin，按照欧拉角(Z₁X₂Z₃)的顺序旋转，即可获得地面坐标系与雷达坐标系之间的旋转矩阵R。

其中，c₁＝cos(yaw),c₂＝cos(pitch),c₃＝cos(qpin)，s₁＝sin(yaw),s₂＝sin(pitch),s₃＝sin(spin)。

可以理解，高度获取装置200在获取完成根据激光雷达300的第三姿态角yaw、第一姿态角pitch、第二姿态角spin以及旋转矩阵R后，高度获取装置200可以根据旋转矩阵R将机器人体坐标系转换为雷达坐标系。

在一些实施例中，若高度获取装置200获取到的坐标不是基于雷达坐标系，而是基于机器人体坐标系或地面坐标系，则可以通过旋转矩阵R将机器人体坐标系或地面坐标系转换为雷达坐标系，再通过转换雷达坐标系下的坐标进行高度计算。

可以理解，高度获取装置200在获取目标物件的坐标时，可能存在不同的数据来源，例如，当高度获取装置200安装在多足机器人100上时，可以通过独立的激光雷达300以及来自传感单元103中的激光雷达扫描获取的坐标，此时两者的坐标不是基于同一个坐标系，因此无法直接进行计算，通过旋转矩阵可以将不同坐标系下的坐标数据进行统一，以提高高度获取装置200的适用性，使得高度获取装置200可以基于不同的数据来源进行高度获取。

在本实施例中，高度获取装置200可以根据高度标定中获取的激光雷达300的第一姿态角pitch、第二姿态角spin获取目标物件的高度。

请一并参阅图8，图8是本申请另一实施方式提供的高度获取装置200的高度获取的示意图。在本实施例中，以激光雷达300为单线激光雷达为例，描述高度获取装置200的工作过程。

在本实施例中，激光雷达300用于扫描目标物件，以获取目标物件在雷达坐标下的坐标。位置获取模块210连接至激光雷达300，位置获取模块210用于获取目标物件的顶点P在雷达坐标系下的第一坐标。距离获取模块220连接位置获取模块210，距离获取模块220用于获取第一坐标，并根据第一坐标获取目标物件的顶点P与雷达坐标系的原点L之间的距离。第一高度获取模块230连接位置获取模块210，第一高度获取模块230用于获取激光雷达300的离地高度H1。姿态角获取模块240用于获取激光雷达300的姿态角。第二高度获取模块250用于根据目标物件的顶点P与雷达坐标系的原点L之间的距离D、激光雷达的离地高度H1、激光雷达的姿态角获取目标物件的高度H2。

具体地，可以通过姿态角获取模块240对激光雷达300进行标定，以获取第一姿态角pitch及第二姿态角spin。位置获取模块210通过激光雷达300扫描目标物件，并获取目标物件的顶点P在雷达坐标系下的第一坐标(x，y，z)。接着，距离获取模块220根据公式(6)获取目标物件的顶点与雷达坐标系的原点L之间的距离。第一高度获取模块230用于获取激光雷达的离地高度H1。第二高度获取模块250再根据公式(7)获取目标物件的离地高度P′H。

P′H＝LG-PL*cos(spin)*sin(pitch) (7)

其中，x，y，z分别为目标物件的顶点P在雷达坐标系内的x轴、y轴及z轴的第一坐标，P为目标物件的顶点，PP′为与oo'平行且与跟LA相交的直线，且交点在P′。

请一并参阅图9，图9是本申请一实施例提供的高度获取示意图。如图9所示，第二高度获取模块250可以通过公式(8)获取目标物件的离地高度H2。

H2＝H1-D*cos(spin)*sin(pitch) (8)

其中，H1为激光雷达300的坐标系原点L的离地高度，D为目标物件的顶点与雷达坐标系的原点L之间的距离，spin为激光雷达300的第二姿态角，pitch为激光雷达300的第一姿态角。

可以理解，当激光雷达300为单线激光雷达时，本申请提供的高度获取装置200通过将单线激光雷达安装在一个预设的离地高度，并获取该离地高度，可以根据单线激光雷达的离地高度及单线激光雷达的姿态角获取目标物件的离地高度，使得原本无法用来测量目标物件高度的单线激光雷达可以用来获取目标物件的高度。

可以理解，当激光雷达300为单线激光雷达时，本申请提供的高度获取装置200可以先获取单线激光雷达的离地高度，再根据单线激光雷达的离地高度以及标定物对单线激光雷达的姿态角进行标定，根据标定后的姿态角和单线激光雷达的离地高度即可获取目标物件的高度。在姿态角标定结束后，在单线激光雷达的离地高度未发生变化时，可以持续对目标物件进行高度获取。在单线激光雷达的离地高度发生变化后，仅需要重新进行姿态角标定即可测量目标物件的高度。本申请提供的高度获取装置200具有操作简单，且适用性强的优势。

在本实施例中，当激光雷达300为多线激光雷达时，可选取多线激光雷达内的第一线束进行标定物的高度标定。以及获取目标物件的高度。

可以理解，当激光雷达300为多线激光雷达时，由于多线激光雷达具有多条扫描线束，因此每条扫描线束都会获取到多个回波角点，在进行高度标定时，需选取第一线束获取到的回波角点进行高度标定。

可以理解，激光雷达300为多线激光雷达与激光雷达300为单线激光雷达的区别在于，当激光雷达300为多线激光雷达时，选取多线激光雷达中的一条扫描线束，使用一条扫描线束的扫描结果来进行标定物的高度标定以及目标物件的高度获取。

可以理解，第一线束可以为多线激光雷达中任意的扫描线束，姿态角获取模块240在进行高度标定时需要获取第一线束的纬度角θ_scanline。例如，纬度角θ_scanline可以为预设角度的扫描线束，如纬度角θ_scanline为0°的扫描线束。

可以理解，当多线激光雷达中纬度角θ_scanline不为0时，获取第一线束的纬度角θ_scanline，根据公式(9)修正第一姿态角pitch以获取修正后的第一姿态角pitch′。

pitch′＝pitch±θ_scanline (9)

其中，θ_scanline为多线激光雷达的纬度角，pitch为第一姿态角，pitch′为修正后的第一姿态角。可以理解，公式(9)中的±取决于纬度角θ_scanline所属的半球，当纬度角θ_scanline处于北半球时，公式(9)中的符号为-，当纬度角θ_scanline处于南半球时，公式(9)中的符号为+。

可以理解，请一并参阅图8，第二姿态角spin的修正方式为将直线AL在XY平面投影线与X轴之间的夹角作为修正后的第二姿态角spin′。第三姿态角raw与单线激光雷达时相同。

具体地，请一并参阅图3至图9，当激光雷达300为多线激光雷达时，姿态角获取模块240还用于对多线激光雷达300进行标定，并通过公式(1)获取第一姿态角pitch，再通过公式(9)对第一姿态角pitch进行修正，以获取修正后的第一姿态角pitch′。具体地，姿态角获取模块240首先获取第一线束与地面的交线OO′，再获取第一线束的纬度角θ_scanline。接着，姿态角获取模块240获取交线OO′上距离雷达坐标系的原点L最近的位置点A′。姿态角获取模块240获取位置点A′在雷达坐标系下的位置点坐标(x2，y2，z2)，距离获取模块220根据位置点坐标(x2，y2，z2)及公式(6)获取位置点A′与雷达坐标系的原点L之间的距离。最后，姿态角获取模块240根据多线激光雷达300的离地高度、位置点A′与雷达坐标系的原点L之间的距离以及第一线束的纬度角θ_scanline，将参数带入公式(1)、(6)及(9)获取修正后的第一姿态角pitch′。

可以理解，当激光雷达300为多线激光雷达时可以通过修正后的第一姿态角pitch′与修正后的第二姿态角spin′替代第一姿态角pitch与第二姿态角spin进行高度运算，具体运算方式请一并参阅图3至图9中激光雷达300为单线激光雷达时的描述，在此不再赘述。

可以理解，当激光雷达300为多线激光雷达时，本申请提供的高度获取装置200通过将多线激光雷达安装在一个预设的离地高度，并获取该离地高度，可以仅通过多线激光雷达中的一条扫描线束获取目标物件的高度，可以减少高度获取时的计算量。

本申请一实施方式还提供一种高度获取方法，请一并参阅图10，图10是本申请一实施例提供的高度获取方法的流程示意图。高度获取方法可以应用于如图3所示的高度获取装置200。可以理解，如图10所示，高度获取方法至少包括以下步骤。

S100：获取目标物件的顶点在雷达坐标系下的第一坐标。

在本申请一实施例中，激光雷达300用于扫描目标物件，以获取目标物件在雷达坐标下的坐标。具体地，位置获取模块210连接至激光雷达300，位置获取模块210用于获取目标物件的顶点在雷达坐标系下的第一坐标。

S200：根据第一坐标获取目标物件的顶点与雷达坐标系的原点之间的距离。

在本实施例中，距离获取模块220用于获取第一坐标，并根据第一坐标获取目标物件的顶点与雷达坐标系的原点之间的距离。

具体地，距离获取模块220可以根据公式(6)获取目标物件的顶点与雷达坐标系的原点L之间的距离。

其中，x，y，z分别为目标物件的顶点在雷达坐标系内的x轴、y轴及z轴坐标，D为目标物件的顶点与雷达坐标系的原点L之间的距离。

S300：获取激光雷达的离地高度。

在本申请一实施例中，第一高度获取模块230用于获取激光雷达300的离地高度。

可以理解，当高度获取装置300作为多足机器人100的一部分时，激光雷达300的离地高度(即雷达坐标系的原点到地面距离)存储在存储单元105中，第一高度获取模块230可以通过读取存储单元105内的数据获取激光雷达300的离地高度。

S400：获取激光雷达的姿态角。

在本申请一实施例中，姿态角获取模块240用于获取激光雷达300的姿态角。在本实施例中，激光雷达300的姿态角至少包括第一姿态角pitch、第二姿态角spin及第三姿态角raw。

具体地，姿态角获取模块240可以通过公式(1)获取激光雷达300的第一姿态角pitch。

其中，pitch为激光雷达300的第一姿态角，H1为激光雷达300的雷达坐标系的原点离地高度，LA为位置点A距离雷达坐标系的原点L的距离。

S500：根据目标物件的顶点与雷达坐标系的原点之间的距离、激光雷达的离地高度、激光雷达的姿态角获取目标物件的高度。

在本申请一实施例中，第二高度获取模块250用于根据目标物件的顶点与雷达坐标系的原点之间的距离、激光雷达的离地高度、激光雷达的姿态角获取目标物件的高度。

具体地，第二高度获取模块250可以通过公式(8)获取目标物件的离地高度H2。

H2＝H1-D*cos(spin)*sin(pitch) (8)

其中，H1为激光雷达300的坐标系原点L的离地高度，D为目标物件的顶点与雷达坐标系的原点L之间的距离，spin为激光雷达300的第二姿态角，pitch为激光雷达300的第一姿态角。

本申请一实施方式还提供一种高度获取方法，请一并参阅图11，图11是本申请一实施例提供的高度获取标定流程示意图。可以理解，高度获取装置200的激光雷达300为多线激光雷达。如图11所示，高度获取标定至少包括以下步骤。

S110：获取多线激光雷达的第一线束与地面的交线。

在本申请一实施例中，当激光雷达300为多线激光雷达时，选取激光雷达300的第一线束扫描目标物件，以获取目标物件在雷达坐标下的坐标。姿态角获取模块240用于获取激光雷达300中第一线束与地面的交线OO′。

具体地，激光雷达300在扫描时，会以自身上的某一点为原点建立一个雷达坐标系。其中，雷达坐标系为右手坐标系，包括x轴、y轴及z轴，x轴对应于激光雷达300的朝向，y轴向激光雷达300的朝向的左侧，z轴朝上。由于激光雷达300安装在预设的离地高度，且激光雷达的扫描方向朝向地面倾斜，因此激光雷达300的扫描线与地面的交线OO′为一条直线。

S210：获取交线上距离雷达坐标系的原点最近的位置点。

在本申请一实施例中，姿态角获取模块240用于获取交线OO′上距离雷达坐标系的原点L最近的位置点A′。可以理解，位置点A′在雷达坐标系中的坐标即为位置点坐标。

S310：获取位置点在雷达坐标系下的位置点坐标。

在本申请一实施例中，姿态角获取模块240获取位置点A′在雷达坐标系下的位置点坐标(x3，y3，z3)。具体地，激光雷达300在进行扫描时，会为扫描到的点标注其在雷达坐标系中的坐标值。

S410：根据位置点坐标获取位置点与雷达坐标系的原点之间的距离。

在本申请一实施例中，姿态角获取模块240可以根据位置点坐标(x3，y3，z3)获取位置点A′与雷达坐标系的原点L之间的距离。具体地，姿态角获取模块240可以根据公式(6)将位置点坐标(x3，y3，z3)代入公式(6)中，以获取位置点A′与雷达坐标系的原点L之间的距离。

S510：根据多线激光雷达的离地高度、位置点与雷达坐标系的原点之间的距离以及第一线束的纬度角获取第一姿态角。

在本申请一实施例中，姿态角获取模块240根据激光雷达300的离地高度H1、位置点A′与雷达坐标系的原点L之间的距离以及第一线束的纬度角θ_scanline，获取激光雷达300为多线激光雷达时的第一姿态角pitch′。

具体地，姿态角获取模块240根据激光雷达300的离地高度H1、位置点A′与雷达坐标系的原点L之间的距离以及第一线束的纬度角θ_scanline，将参数带入公式(1)、(6)及(9)获取修正后的第一姿态角pitch′。

本申请实施例还提供一种计算机可读存储介质，计算机可读存储介质包括计算机程序。当计算机程序在高度获取装置200上运行时，使得高度获取装置200执行本申请实施例提供的高度获取方法。

本申请实施例提供的高度获取装置200、高度获取方法及计算机可读存储介质，通过激光雷达300的离地高度及激光雷达的姿态角获取目标物件顶点的离地高度，可以适用于单线激光雷达和多线激光雷达，在应用于单线激光雷达时可以使单线激光雷达能够测量离地高度，当应用于多线激光雷达时，可以仅通过一条扫描线束获取目标物件的高度，可以减少高度获取时的计算量。

上面结合附图对本申请实施例作了详细说明，但是本申请不限于上述实施例，在所属技术领域普通技术人员所具备的知识范围内，还可以在不脱离本申请宗旨的前提下做出各种变化。

Claims (10)

1.一种高度获取装置，应用于激光雷达，其特征在于，所述高度获取装置用于获取目标物件的高度，所述高度获取装置包括：

位置获取模块，所述位置获取模块用于获取所述目标物件的顶点在雷达坐标系下的第一坐标；

距离获取模块，所述距离获取模块连接所述位置获取模块，所述距离获取模块用于根据所述第一坐标获取所述目标物件的顶点与所述雷达坐标系的原点之间的距离；

第一高度获取模块，所述第一高度获取模块连接所述位置获取模块，所述第一高度获取模块用于获取所述激光雷达的离地高度；

姿态角获取模块，所述姿态角获取模块用于获取所述激光雷达的姿态角；

第二高度获取模块，所述第二高度获取模块连接所述距离获取模块、所述第一高度获取模块及所述姿态角获取模块，所述第二高度获取模块用于根据所述目标物件的顶点与所述雷达坐标系的原点之间的距离、所述激光雷达的离地高度、所述激光雷达的姿态角获取所述目标物件的高度。

2.如权利要求1所述的高度获取装置，其特征在于，所述激光雷达的姿态角包括所述激光雷达的第一姿态角及第二姿态角；

所述第二高度获取模块用于根据以下公式获取所述目标物件的高度H2：

H2＝H1-D*cos(spin)*sin(pitch)；

其中，H1为所述激光雷达的离地高度，D为所述目标物件的顶点与所述雷达坐标系的原点之间的距离，pitch为所述第一姿态角，spin为所述第二姿态角。

3.如权利要求2所述的高度获取装置，其特征在于，所述激光雷达包括单线激光雷达或者多线激光雷达。

4.如权利要求3所述的高度获取装置，其特征在于，当所述激光雷达为所述单线激光雷达时，所述姿态角获取模块还用于：

对所述单线激光雷达进行标定以获取所述第一姿态角；其中，所述对所述单线激光雷达进行标定以获取所述第一姿态角，包括：

获取所述单线激光雷达的扫描线与地面的交线；

获取所述交线上距离所述雷达坐标系的原点最近的位置点；

获取所述位置点在所述雷达坐标系下的位置点坐标；

根据所述位置点坐标获取所述位置点与所述雷达坐标系的原点之间的距离；

根据所述单线激光雷达的离地高度、所述位置点与所述雷达坐标系的原点之间的距离获取所述第一姿态角。

5.如权利要求3所述的高度获取装置，其特征在于，当所述激光雷达为所述多线激光雷达时，所述多线激光雷达用于应用第一线束扫描目标物件；

所述姿态角获取模块还用于：对所述多线激光雷达进行标定以获取所述第一姿态角；其中，所述对所述多线激光雷达进行标定以获取所述第一姿态角，包括：

获取所述第一线束与地面的交线；

获取所述第一线束的纬度角；

获取所述交线上距离所述雷达坐标系的原点最近的位置点；

获取所述位置点在所述雷达坐标系下的位置点坐标；

根据所述位置点坐标获取所述位置点与所述雷达坐标系的原点之间的距离；

根据所述多线激光雷达的离地高度、所述位置点与所述雷达坐标系的原点之间的距离以及所述第一线束的纬度角获取所述第一姿态角。

6.如权利要求4或5所述的高度获取装置，其特征在于，所述姿态角获取模块还用于：

对所述激光雷达进行标定以获取所述第二姿态角；其中，所述对所述激光雷达进行标定以获取所述第二姿态角，包括：

根据所述位置点在所述雷达坐标系下的所述位置点坐标以及所述雷达坐标系的原点坐标确定第一向量；

获取所述第一向量与所述雷达坐标系的X轴之间的第一夹角；

所述第一夹角为所述第二姿态角。

7.如权利要求3所述的高度获取装置，其特征在于，所述激光雷达的姿态角还包括所述激光雷达的第三姿态角：

对所述激光雷达进行标定以获取所述第三姿态角；其中，所述对所述激光雷达进行标定以获取所述第三姿态角，包括：

响应于标定物和所述激光雷达发生相对移动，通过所述激光雷达扫描所述标定物，获取发生相对移动前所述标定物的标定点在所述雷达坐标系下的第一标定坐标以及发生相对移动后标定物的标定点在所述雷达坐标系下的第二标定坐标；

根据所述第一标定坐标以及所述第二标定坐标确定第二向量；

获取所述第一向量和所述第二向量之间的第二夹角；

根据所述第二夹角以及所述第一姿态角获取所述第三姿态角。

8.如权利要求7所述的高度获取装置，其特征在于，所述标定物包括直角棱柱或三角棱柱；

扫描所述直角棱柱或所述三角棱柱以获取回波角点，所述回波角点为所述标定点。

9.一种高度获取方法，应用于激光雷达，其特征在于，所述高度获取方法包括：

获取目标物件的顶点在雷达坐标系下的第一坐标；

根据所述第一坐标获取所述目标物件的顶点与所述雷达坐标系的原点之间的距离；

获取所述激光雷达的离地高度；

获取所述激光雷达的姿态角；

根据所述目标物件的顶点与所述雷达坐标系的原点之间的距离、所述激光雷达的离地高度、所述激光雷达的姿态角获取所述目标物件的高度。

10.一种计算机可读存储介质，其特征在于，所述计算机存储介质存储有计算机程序，当所述计算机程序被处理器执行时，使所述处理器执行如权利要求9所述的高度获取方法。

Images