CN116300613A - 一种融合uwb和imu的智能跟随服务机器人及其控制方法、装置和存储介质 - Google Patents
一种融合uwb和imu的智能跟随服务机器人及其控制方法、装置和存储介质 Download PDFInfo
- Publication number
- CN116300613A CN116300613A CN202310251386.2A CN202310251386A CN116300613A CN 116300613 A CN116300613 A CN 116300613A CN 202310251386 A CN202310251386 A CN 202310251386A CN 116300613 A CN116300613 A CN 116300613A
- Authority
- CN
- China
- Prior art keywords
- state quantity
- system state
- service robot
- uwb
- intelligent
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 32
- 238000012545 processing Methods 0.000 claims abstract description 23
- 230000008859 change Effects 0.000 claims description 13
- 239000011159 matrix material Substances 0.000 claims description 12
- 230000008569 process Effects 0.000 claims description 8
- 238000005259 measurement Methods 0.000 claims description 7
- 238000004364 calculation method Methods 0.000 claims description 4
- 230000033001 locomotion Effects 0.000 abstract description 6
- 238000004590 computer program Methods 0.000 description 8
- 230000006870 function Effects 0.000 description 6
- 230000000694 effects Effects 0.000 description 5
- 238000004422 calculation algorithm Methods 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000002265 prevention Effects 0.000 description 2
- 230000003321 amplification Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 239000012141 concentrate Substances 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000003384 imaging method Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000003199 nucleic acid amplification method Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000012549 training Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B19/00—Programme-control systems
- G05B19/02—Programme-control systems electric
- G05B19/04—Programme control other than numerical control, i.e. in sequence controllers or logic controllers
- G05B19/042—Programme control other than numerical control, i.e. in sequence controllers or logic controllers using digital processors
- G05B19/0423—Input/output
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/20—Pc systems
- G05B2219/25—Pc structure of the system
- G05B2219/25257—Microcontroller
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
- Y02D30/00—Reducing energy consumption in communication networks
- Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks
Landscapes
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种融合UWB和IMU的智能跟随服务机器人及其控制方法、装置和存储介质，包括信号接收模块、处理模块和驱动模块。信号接收模块接收被跟随目标发出的UWB数据和IMU数据，处理模块融合UWB数据和IMU数据进行非线性坐标估计，确定预测系统状态量，驱动机器人按照预测系统状态量进行动作，对被跟随目标进行跟随。本发明通过探测被跟随目标的UWB数据和IMU数据并进行融合，实现成本较低，受到环境的干扰较小，能够提高机器人的稳定性；根据UWB数据和IMU数据进行非线性坐标估计确定预测系统状态量，能够控制机器人适应被跟随目标的非线性运动，提高机器人的跟随成功率。本发明广泛应用于机器人技术领域。
Description
技术领域
本发明涉及机器人技术领域，尤其是一种融合UWB和IMU的智能跟随服务机器人及其控制方法、装置和存储介质。
背景技术
现今，智能机器人在各个应用场景中得到了越来越广泛的应用，其中跟随服务机器人具有跟随目标的功能，能够应用在工业辅助、康复训练、卫生防疫以及物流运输等场合。例如，在卫生防疫场合，跟随服务机器人可以对医疗人员进行智能跟随，提供24小时不间断的贴身服务，仅可以辅助医务人员执行日常的物品运输任务，从而提高医疗效率和便利性，还可以帮助医务人员分担工作，让他们能更专注于提供高质量的医疗服务。
跟随服务机器人的核心功能是跟随功能。目前的相关技术中，机器人使用激光雷达或者相机等设备对自身和目标进行定位，从而实现对目标的跟随，其中激光雷达的成本较高，而无论是激光雷达还是相机，都容易受环境影响而降低效果。
术语解释：
UWB：Ultra Wide Band，即超宽带，可以应用于通信和定位；
IMU：Inertial Measurement Unit，即惯性测量单元，它由三个单轴的加速度计和三个单轴的陀螺仪组成，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺仪检测载体相对于导航坐标系的角速度信号，对这些信号进行处理之后，便可解算出物体的姿态。
发明内容
针对目前的跟随服务机器人的实现成本较高，以及容易受环境影响而降低效果等技术问题，本发明的目的在于提供一种融合UWB和IMU的智能跟随服务机器人及其控制方法、装置和存储介质。
一方面，本发明实施例包括一种融合UWB和IMU的智能跟随服务机器人，所述融合UWB和IMU的智能跟随服务机器人包括：
信号接收模块；所述信号接收模块包括若干个UWB基站，所述信号接收模块用于通过各所述UWB基站接收被跟随目标发出的UWB数据，以及用于接收被跟随目标发出的IMU数据；
处理模块；所述处理模块用于融合所述UWB数据和所述IMU数据进行非线性坐标估计，确定预测系统状态量；
驱动模块；所述驱动模块用于驱动智能跟随服务机器人按照所述预测系统状态量进行动作，对被跟随目标进行跟随。
进一步地，所述融合所述UWB数据和所述IMU数据进行非线性坐标估计，确定预测系统状态量，包括：
根据所述IMU数据，确定被跟随目标的姿态角；
根据所述UWB数据，确定智能跟随服务机器人与被跟随目标之间的距离值以及距离变化值；
获取智能跟随服务机器人的第一系统状态量；所述第一系统状态量为智能跟随服务机器人在第一时刻的系统状态量；
根据所述第一系统状态量和所述姿态角，确定第二系统状态量；所述第二系统状态量为智能跟随服务机器人在第二时刻的系统状态量，所述第二时刻为所述第一时刻之后的一个时刻；
根据所述第一系统状态量、所述姿态角、所述距离值以及所述距离变化值，确定观测值与预测值之差；
根据所述第一系统状态量，确定雅可比式；
根据所述第二系统状态量和所述雅可比式，确定卡尔曼增益；
根据所述第二系统状态量、所述雅可比式、所述卡尔曼增益以及所述观测值与预测值之差，确定所述预测系统状态量。
进一步地，所述根据所述第一系统状态量和所述姿态角，确定第二系统状态量，包括：
根据公式
进行计算；其中，
为所述第二系统状态量，p′x和p′y为智能跟随服务机器人在所述第二时刻的位置坐标，v′x和v′y为智能跟随服务机器人在所述第二时刻的速度，Δt为所述第一时刻与所述第二时刻之间的时长，θ为所述姿态角，/>
为所述第一系统状态量，px和py为智能跟随服务机器人在所述第一时刻的位置坐标，vx和vy为智能跟随服务机器人在所述第一时刻的速度，
为过程噪声。
进一步地，所述根据所述第一系统状态量、所述姿态角、所述距离值以及所述距离变化值，确定观测值与预测值之差，包括：
根据公式
进行计算；其中，m为所述观测值与预测值之差，ρ为所述距离值，/>
为所述姿态角中的偏航角，/>
为所述距离变化值。
进一步地，所述根据所述第一系统状态量，确定雅可比式，包括：
根据公式
进行计算；其中，H为所述雅可比式。
进一步地，所述根据所述第二系统状态量和所述雅可比式，确定卡尔曼增益，包括：
根据公式K＝P′HTS-1进行计算；其中，S＝HP′HT+R，K为所述卡尔曼增益，P′为所述第二系统状态量
的误差协方差矩阵，HT为所述雅可比式H的转置矩阵，R为测量噪声矩阵。
进一步地，所述根据所述第二系统状态量、所述雅可比式、所述卡尔曼增益以及所述观测值与预测值之差，确定所述预测系统状态量，包括：
根据公式x″＝x′+Km进行计算；其中，x″为所述预测系统状态量。
另一方面，本发明实施例还包括一种融合UWB和IMU的智能跟随服务机器人的控制方法，所述控制方法包括：
接收被跟随目标发出的UWB数据和IMU数据；
融合所述UWB数据和所述IMU数据进行非线性坐标估计，确定被跟随目标的预测位置；
根据所述预测位置，驱动智能跟随服务机器人跟随被跟随目标。
另一方面，本发明实施例还包括一种计算机装置，包括存储器和处理器，所述存储器用于存储至少一个程序，所述处理器用于加载所述至少一个程序以执行实施例中的融合UWB和IMU的智能跟随服务机器人的控制方法。
另一方面，本发明实施例还包括一种存储介质，其中存储有处理器可执行的程序，所述处理器可执行的程序在由处理器执行时用于执行实施例中的融合UWB和IMU的智能跟随服务机器人的控制方法。
本发明的有益效果是：实施例中的融合UWB和IMU的智能跟随服务机器人，通过探测被跟随目标的UWB数据和IMU数据并进行融合，由于UWB天线、UWB基站以及IMU传感器的成本较低，因此实现成本较低；与激光雷达和相机等设备相比，UWB数据和IMU数据的检测和传输受到环境的干扰较小，因此能够提高机器人的稳定性；根据UWB数据和IMU数据进行非线性坐标估计从而确定预测系统状态量，能够控制机器人适应被跟随目标的非线性运动，从而提高机器人的跟随成功率。
附图说明
图1为实施例中融合UWB和IMU的智能跟随服务机器人的结构图；
图2为实施例中计算出机器人与被跟随目标之间的距离值以及距离变化值的原理图；
图3为实施例中智能跟随服务机器人的控制方法的流程图。
具体实施方式
本实施例中，融合UWB和IMU的智能跟随服务机器人的结构如图1所示，包括信号接收模块、处理模块和驱动模块。
其中，信号接收模块内置一个单线激光雷达、一个6轴IMU传感器和两个UWB接收器(UWB基站)。在使用融合UWB和IMU的智能跟随服务机器人时，由需要被机器人跟随的目标即被跟随目标携带或者安装一个设备，该设备内置一个6轴IMU传感器和一个UWB发送器。被跟随目标所携带的6轴IMU传感器测得被跟随目标的姿态角等运动数据并生成IMU数据，被跟随目标所携带的UWB发送器测得被跟随目标的位置坐标并生成UWB数据，对外发送IMU数据和UWB数据。智能跟随服务机器人中的信号接收模块接收被跟随目标发出的UWB数据和IMU数据。
处理模块的数据输入端与信号接收模块连接，处理模块的数据输出端与驱动模块连接。信号接收模块将接收到的UWB数据和IMU数据发送至处理模块，由处理模块进行处理。处理模块融合UWB数据和IMU数据后，进行非线性坐标估计，从而确定预测系统状态量，然后生成预测系统状态量对应的控制指令，将控制指令发送至驱动模块，由驱动模块驱动智能跟随服务机器人按照预测系统状态量进行动作，对被跟随目标进行跟随。
本实施例中，智能跟随服务机器人通过探测被跟随目标的UWB数据和IMU数据并进行融合，由于UWB天线、UWB基站以及IMU传感器的成本较低，因此实现成本较低；与激光雷达和相机等设备相比，UWB数据和IMU数据的检测和传输受到环境的干扰较小，因此能够提高机器人的稳定性；根据UWB数据和IMU数据进行非线性坐标估计从而确定预测系统状态量，能够控制机器人适应被跟随目标的非线性运动，从而提高机器人的跟随成功率。
本实施例中，处理模块在进行融合UWB数据和IMU数据进行非线性坐标估计，确定预测系统状态量时，具体可以执行以下步骤：
S201.根据IMU数据，确定被跟随目标的姿态角；
S202.根据UWB数据，确定智能跟随服务机器人与被跟随目标之间的距离值以及距离变化值；
S203.获取智能跟随服务机器人的第一系统状态量；第一系统状态量为智能跟随服务机器人在第一时刻的系统状态量；
S204.根据第一系统状态量和姿态角，确定第二系统状态量；第二系统状态量为智能跟随服务机器人在第二时刻的系统状态量，第二时刻为第一时刻之后的一个时刻；
S205.根据第一系统状态量、姿态角、距离值以及距离变化值，确定观测值与预测值之差；
S206.根据第一系统状态量，确定雅可比式；
S207.根据第二系统状态量和雅可比式，确定卡尔曼增益；
S208.根据第二系统状态量、雅可比式、卡尔曼增益以及观测值与预测值之差，确定预测系统状态量。
步骤S201中，处理模块根据被跟随目标发出的IMU数据，可以计算出被跟随目标的姿态角θ。具体地，姿态角θ的具体形式可以是偏航角、俯仰角和滚转角等，本实施例中，使用偏航角作为姿态角进行说明。具体地，通过公式
计算得到被跟随目标的偏航角/>
其中，q0，q1，q2，q3是四元数，初始值分别设置为1，0，0，0。可以经过以下步骤获取和更新四元数：
1)四元数表示重力向量：
2)求误差：
a是线加速度测量值。
3)利用误差纠正陀螺仪：
利用PI积分纠正：
其中，KP是放大系数，KI是积分比例系数，
是陀螺仪的角速度(弧度表示)测量值。
4)更新四元数：
T是IMU采样频率。
步骤S202中，处理模块根据被跟随目标发出的UWB数据，可以计算出智能跟随服务机器人与被跟随目标之间的距离值以及距离变化值。步骤S202的原理如图2所示，其中MS为被跟随目标携带的UWB发送器(UWB标签)，UWB1和UWB2为智能跟随服务机器人的信号接收模块中的两个UWB接收器(UWB基站)。
步骤S202中，参照图2，在处理模块建立的坐标系中，UWB1和UWB2两个UWB基站的位置是已知的，UWB发送器发送的信号到达UWB1和UWB2这两个已定位的节点时，可以确定UWB1到UWB发送器之间连线与基准方向的夹角α1，并在此方向上绘制出一条射线L1；同样，UWB2到UWB发送器之间的连线与基准方向的夹角α2也可以确定，并在此方向上绘制出一条射线L2。那么射线L1与射线L2的交点就是UWB发送器的到达角位置。
设基站UWB1的坐标记作(x1,y1)，UWB2的坐标记作(x2,y2)，被跟随目标(UWB发送器)的坐标为(x,y)，假设α1和α2均不为90°，则射线L1的直线方程为y-y1＝k1(x-x1)，射线L2的直线方程为y-y2＝k2(x-x2)，其中k1＝tan(α1)，k2＝tan(α2)，通过求解这两条直线的交点，可以确定被跟随目标(UWB发送器)的到达角位置。求解射线L1和L2的交点坐标：
解得：
所求的x，y即是被跟随目标(UWB发送器)的位置坐标。
步骤S202中，对被跟随目标和机器人进行质点化处理。被跟随目标(UWB发送器)的位置坐标(x，y)已知，而处理模块可以通过对机器人进行自身定位，获得机器人的坐标，因此可以计算出智能跟随服务机器人与被跟随目标之间的距离值。本实施例中，将在k时刻(第一时刻)计算出的智能跟随服务机器人与被跟随目标之间的距离值记为ρ，将在k+1时刻(第二时刻)计算出的智能跟随服务机器人与被跟随目标之间的距离值记为ρ′，k时刻(第一时刻)与k+1时刻(第二时刻)之间的时长为Δt，那么从k时刻(第一时刻)到k+1时刻(第二时刻)，智能跟随服务机器人与被跟随目标之间的距离值以及距离变化值为
步骤S203中，处理模块对k时刻(第一时刻)智能跟随服务机器人的位置和速度进行检测，获得智能跟随服务机器人在k时刻(第一时刻)的位置坐标px和py以及速度vx和vy，这些参数组成智能跟随服务机器人在k时刻(第一时刻)的系统状态量，即第一系统状态量
本实施例中，被跟随目标可能做非线性运动，因此可以使用扩展卡尔曼滤波算法，对系统状态量进行滤波，从而获得更精准的状态量。扩展卡尔曼滤波算法的实现步骤包括步骤S204-S208。
步骤S204中，处理模块根据第一系统状态量
和姿态角θ(具体地姿态角θ可以是偏航角/>
)，建立机器人的运动学模型
从而计算得到智能跟随服务机器人在k+1时刻(第二时刻)的系统状态量，即第二系统状态量/>
步骤S204的运动学模型中，
为状态转移矩阵，因此运动学模型也可以表示为Pk′＝FPk-1FT+Q。其中，/>
代表过程噪声，在实际计算时可以将Q置为0。
步骤S205中，根据第一系统状态量
中的位置坐标px、py和速度vx、vy，姿态角θ(具体可以取为偏航角/>
)、距离值ρ以及距离变化值/>
根据公式/>
计算得到观测值与预测值之差m，其中/>
本实施例中，由于函数h(x′)是非线性的，将函数h(x′)在泰勒公式原点展开，忽略二次以上的高阶项，可以获得雅克比式：
由雅可比式H的计算公式可知，步骤S206中，可以根据第一系统状态量
中的位置坐标px、py和速度vx、vy等参数计算得到雅可比式H。
步骤S207中，通过公式K＝P′HTS-1计算得到卡尔曼增益K。其中，S＝HP′HT+R，P′为第二系统状态量
的误差协方差矩阵，HT为雅可比式H的转置矩阵，R为测量噪声矩阵，测量噪声矩阵R是传感器的内参。
步骤S208中，根据第二系统状态量x′、雅可比式H、卡尔曼增益K以及观测值与预测值之差m，根据公式x″＝x′+Km计算得到预测系统状态量x″，同时根据公式P”＝(1-KH)P’进行计算，其中P”为预测系统状态量x″对应的误差协方差矩阵。
通过执行步骤S201-S208，实现了使用扩展卡尔曼滤波算法对系统状态量进行滤波，从而获得精准的预测系统状态量x″。预测系统状态量x″所包括的参数的量纲与第一系统状态量和第二系统状态量等相同。预测系统状态量x″能够指示机器人将要执行的动作，使得机器人实现预测系统状态量x″对应的位置坐标和速度等参数，从而使机器人能够应对被跟随目标做出的非线性运动，能够有效对被跟随目标进行跟随。
本实施例中，处理模块根据预测系统状态量x″生成控制指令，将控制指令发送至驱动模块，从而控制驱动模块中的电机等部件动作，驱动机器人实现预测系统状态量x″对应的位置坐标和速度等参数。例如，处理模块可以根据预测系统状态量x″中的位置坐标设定一个导航点，计算并规划出从机器人当前位置到导航点的最优全局路径，根据最优全局路径，规划局部路径，生成控制指令，控制驱动模块驱动机器人沿着局部路径动作。
本实施例中，参照图3，融合UWB和IMU的智能跟随服务机器人的控制方法包括以下步骤：
S1.接收被跟随目标发出的UWB数据和IMU数据；
S2.融合UWB数据和IMU数据进行非线性坐标估计，确定被跟随目标的预测位置；
S3.根据预测位置，驱动智能跟随服务机器人跟随被跟随目标。
其中，步骤S1可以由本实施例中融合UWB和IMU的智能跟随服务机器人的信号接收模块执行，步骤S2可以由处理模块执行，步骤S3可以由驱动模块执行。通过执行步骤S1-S3，可以实现对本实施例中融合UWB和IMU的智能跟随服务机器人的控制，从而实现本实施例中融合UWB和IMU的智能跟随服务机器人的技术效果。
可以通过编写执行本实施例中的融合UWB和IMU的智能跟随服务机器人的控制方法的计算机程序，将该计算机程序写入至存储介质或者计算机装置中，当计算机程序被读取出来运行时，执行本实施例中的融合UWB和IMU的智能跟随服务机器人的控制方法，从而实现与实施例中的融合UWB和IMU的智能跟随服务机器人的控制方法相同的技术效果。
需要说明的是，如无特殊说明，当某一特征被称为“固定”、“连接”在另一个特征，它可以直接固定、连接在另一个特征上，也可以间接地固定、连接在另一个特征上。此外，本公开中所使用的上、下、左、右等描述仅仅是相对于附图中本公开各组成部分的相互位置关系来说的。在本公开中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。此外，除非另有定义，本实施例所使用的所有的技术和科学术语与本技术领域的技术人员通常理解的含义相同。本实施例说明书中所使用的术语只是为了描述具体的实施例，而不是为了限制本发明。本实施例所使用的术语“和/或”包括一个或多个相关的所列项目的任意的组合。
应当理解，尽管在本公开可能采用术语第一、第二、第三等来描述各种元件，但这些元件不应限于这些术语。这些术语仅用来将同一类型的元件彼此区分开。例如，在不脱离本公开范围的情况下，第一元件也可以被称为第二元件，类似地，第二元件也可以被称为第一元件。本实施例所提供的任何以及所有实例或示例性语言(“例如”、“如”等)的使用仅意图更好地说明本发明的实施例，并且除非另外要求，否则不会对本发明的范围施加限制。
应当认识到，本发明的实施例可以由计算机硬件、硬件和软件的组合、或者通过存储在非暂时性计算机可读存储器中的计算机指令来实现或实施。所述方法可以使用标准编程技术-包括配置有计算机程序的非暂时性计算机可读存储介质在计算机程序中实现，其中如此配置的存储介质使得计算机以特定和预定义的方式操作——根据在具体实施例中描述的方法和附图。每个程序可以以高级过程或面向对象的编程语言来实现以与计算机系统通信。然而，若需要，该程序可以以汇编或机器语言实现。在任何情况下，该语言可以是编译或解释的语言。此外，为此目的该程序能够在编程的专用集成电路上运行。
此外，可按任何合适的顺序来执行本实施例描述的过程的操作，除非本实施例另外指示或以其他方式明显地与上下文矛盾。本实施例描述的过程(或变型和/或其组合)可在配置有可执行指令的一个或多个计算机系统的控制下执行，并且可作为共同地在一个或多个处理器上执行的代码(例如，可执行指令、一个或多个计算机程序或一个或多个应用)、由硬件或其组合来实现。所述计算机程序包括可由一个或多个处理器执行的多个指令。
进一步，所述方法可以在可操作地连接至合适的任何类型的计算平台中实现，包括但不限于个人电脑、迷你计算机、主框架、工作站、网络或分布式计算环境、单独的或集成的计算机平台、或者与带电粒子工具或其它成像装置通信等等。本发明的各方面可以以存储在非暂时性存储介质或设备上的机器可读代码来实现，无论是可移动的还是集成至计算平台，如硬盘、光学读取和/或写入存储介质、RAM、ROM等，使得其可由可编程计算机读取，当存储介质或设备由计算机读取时可用于配置和操作计算机以执行在此所描述的过程。此外，机器可读代码，或其部分可以通过有线或无线网络传输。当此类媒体包括结合微处理器或其他数据处理器实现上文所述步骤的指令或程序时，本实施例所述的发明包括这些和其他不同类型的非暂时性计算机可读存储介质。当根据本发明所述的方法和技术编程时，本发明还包括计算机本身。
计算机程序能够应用于输入数据以执行本实施例所述的功能，从而转换输入数据以生成存储至非易失性存储器的输出数据。输出信息还可以应用于一个或多个输出设备如显示器。在本发明优选的实施例中，转换的数据表示物理和有形的对象，包括显示器上产生的物理和有形对象的特定视觉描绘。
以上所述，只是本发明的较佳实施例而已，本发明并不局限于上述实施方式，只要其以相同的手段达到本发明的技术效果，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。在本发明的保护范围内其技术方案和/或实施方式可以有各种不同的修改和变化。
Claims (10)
1.一种融合UWB和IMU的智能跟随服务机器人，其特征在于，所述融合UWB和IMU的智能跟随服务机器人包括：
信号接收模块；所述信号接收模块包括若干个UWB基站，所述信号接收模块用于通过各所述UWB基站接收被跟随目标发出的UWB数据，以及用于接收被跟随目标发出的IMU数据；
处理模块；所述处理模块用于融合所述UWB数据和所述IMU数据进行非线性坐标估计，确定预测系统状态量；
驱动模块；所述驱动模块用于驱动智能跟随服务机器人按照所述预测系统状态量进行动作，对被跟随目标进行跟随。
2.根据权利要求1所述的融合UWB和IMU的智能跟随服务机器人，其特征在于，所述融合所述UWB数据和所述IMU数据进行非线性坐标估计，确定预测系统状态量，包括：
根据所述IMU数据，确定被跟随目标的姿态角；
根据所述UWB数据，确定智能跟随服务机器人与被跟随目标之间的距离值以及距离变化值；
获取智能跟随服务机器人的第一系统状态量；所述第一系统状态量为智能跟随服务机器人在第一时刻的系统状态量；
根据所述第一系统状态量和所述姿态角，确定第二系统状态量；所述第二系统状态量为智能跟随服务机器人在第二时刻的系统状态量，所述第二时刻为所述第一时刻之后的一个时刻；
根据所述第一系统状态量、所述姿态角、所述距离值以及所述距离变化值，确定观测值与预测值之差；
根据所述第一系统状态量，确定雅可比式；
根据所述第二系统状态量和所述雅可比式，确定卡尔曼增益；
根据所述第二系统状态量、所述雅可比式、所述卡尔曼增益以及所述观测值与预测值之差，确定所述预测系统状态量。
3.根据权利要求2所述的融合UWB和IMU的智能跟随服务机器人，其特征在于，所述根据所述第一系统状态量和所述姿态角，确定第二系统状态量，包括：
根据公式
进行计算；其中，/>
为所述第二系统状态量，p′x和p′y为智能跟随服务机器人在所述第二时刻的位置坐标，v′x和v′y为智能跟随服务机器人在所述第二时刻的速度，Δt为所述第一时刻与所述第二时刻之间的时长，θ为所述姿态角，/>
为所述第一系统状态量，ox和py为智能跟随服务机器人在所述第一时刻的位置坐标，vx和vy为智能跟随服务机器人在所述第一时刻的速度，/>
为过程噪声。
4.根据权利要求3所述的融合UWB和IMU的智能跟随服务机器人，其特征在于，所述根据所述第一系统状态量、所述姿态角、所述距离值以及所述距离变化值，确定观测值与预测值之差，包括：
根据公式
进行计算；其中，m为所述观测值与预测值之差，ρ为所述距离值，/>
为所述姿态角中的偏航角，/>
为所述距离变化值。
5.根据权利要求4所述的融合UWB和IMU的智能跟随服务机器人，其特征在于，所述根据所述第一系统状态量，确定雅可比式，包括：
根据公式
进行计算；其中，H为所述雅可比式。
6.根据权利要求5所述的融合UWB和IMU的智能跟随服务机器人，其特征在于，所述根据所述第二系统状态量和所述雅可比式，确定卡尔曼增益，包括：
根据公式K＝P′HTS-1进行计算；其中，S＝HP′HT+R，K为所述卡尔曼增益，P′为所述第二系统状态量
的误差协方差矩阵，HT为所述雅可比式H的转置矩阵，R为测量噪声矩阵。
7.根据权利要求6所述的融合UWB和IMU的智能跟随服务机器人，其特征在于，所述根据所述第二系统状态量、所述雅可比式、所述卡尔曼增益以及所述观测值与预测值之差，确定所述预测系统状态量，包括：
根据公式x″＝x′+Km进行计算；其中，x″为所述预测系统状态量。
8.一种融合UWB和IMU的智能跟随服务机器人的控制方法，其特征在于，所述控制方法包括：
接收被跟随目标发出的UWB数据和IMU数据；
融合所述UWB数据和所述IMU数据进行非线性坐标估计，确定被跟随目标的预测位置；
根据所述预测位置，驱动智能跟随服务机器人跟随被跟随目标。
9.一种计算机装置，其特征在于，包括存储器和处理器，所述存储器用于存储至少一个程序，所述处理器用于加载所述至少一个程序以执行权利要求8所述的融合UWB和IMU的智能跟随服务机器人的控制方法。
10.一种计算机可读存储介质，其中存储有处理器可执行的程序，其特征在于，所述处理器可执行的程序在由处理器执行时用于执行权利要求8所述的融合UWB和IMU的智能跟随服务机器人的控制方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310251386.2A CN116300613A (zh) | 2023-03-15 | 2023-03-15 | 一种融合uwb和imu的智能跟随服务机器人及其控制方法、装置和存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310251386.2A CN116300613A (zh) | 2023-03-15 | 2023-03-15 | 一种融合uwb和imu的智能跟随服务机器人及其控制方法、装置和存储介质 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116300613A true CN116300613A (zh) | 2023-06-23 |
Family
ID=86797403
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310251386.2A Pending CN116300613A (zh) | 2023-03-15 | 2023-03-15 | 一种融合uwb和imu的智能跟随服务机器人及其控制方法、装置和存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116300613A (zh) |
-
2023
- 2023-03-15 CN CN202310251386.2A patent/CN116300613A/zh active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108051002B (zh) | 基于惯性测量辅助视觉的运输车空间定位方法及系统 | |
| CN104811683B (zh) | 用于估计位置的方法和设备 | |
| KR101439921B1 (ko) | 비젼 센서 정보와 모션 센서 정보를 융합한 모바일 로봇용 ｓｌａｍ 시스템 | |
| CN108955688A (zh) | 双轮差速移动机器人定位方法及系统 | |
| WO2020048623A1 (en) | Estimation of a pose of a robot | |
| US9122278B2 (en) | Vehicle navigation | |
| Agarwal et al. | Exploiting sensor fusion for mobile robot localization | |
| AU2012260626A1 (en) | Vehicle navigation | |
| CN110942474A (zh) | 机器人目标跟踪方法、设备及存储介质 | |
| EP2527943A1 (en) | Vehicle navigation | |
| Karras et al. | Localization of an underwater vehicle using an IMU and a laser-based vision system | |
| CN113218389B (zh) | 一种车辆定位方法、装置、存储介质及计算机程序产品 | |
| Lin et al. | Proprioceptive invariant robot state estimation | |
| CN114593735A (zh) | 一种位姿预测方法及装置 | |
| Berntorp et al. | Sensor fusion for motion estimation of mobile robots with compensation for out-of-sequence measurements | |
| Zhao et al. | Robust depth-aided visual-inertial-wheel odometry for mobile robots | |
| US11926038B2 (en) | Information processing apparatus and information processing method | |
| Seung et al. | Precise positioning algorithm development for quadrotor flying robots using dual extended Kalman filter | |
| Park et al. | A novel line of sight control system for a robot vision tracking system, using vision feedback and motion-disturbance feedforward compensation | |
| CN116300613A (zh) | 一种融合uwb和imu的智能跟随服务机器人及其控制方法、装置和存储介质 | |
| Park et al. | High performance vision tracking system for mobile robot using sensor data fusion with kalman filter | |
| CN114252073B (zh) | 一种机器人姿态数据融合方法 | |
| Lee et al. | Vision tracking of a moving robot from a second moving robot using both relative and absolute position referencing methods | |
| Low et al. | Visual based SLAM using modified PSO | |
| JP6516332B2 (ja) | 角速度センサを用いてユーザの前方方向を推定する携帯端末、プログラム及び方法 |
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 |