CN111722257A - 一种基于gps和ins组合制导的远程救援机器人系统 - Google Patents
一种基于gps和ins组合制导的远程救援机器人系统 Download PDFInfo
- Publication number
- CN111722257A CN111722257A CN202010603813.5A CN202010603813A CN111722257A CN 111722257 A CN111722257 A CN 111722257A CN 202010603813 A CN202010603813 A CN 202010603813A CN 111722257 A CN111722257 A CN 111722257A
- Authority
- CN
- China
- Prior art keywords
- gps
- point
- vehicle
- error
- positioning
- 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
- 238000001514 detection method Methods 0.000 claims abstract description 19
- 238000000034 method Methods 0.000 claims abstract description 18
- 238000001914 filtration Methods 0.000 claims abstract description 8
- 230000004927 fusion Effects 0.000 claims abstract description 5
- 230000008054 signal transmission Effects 0.000 claims abstract description 4
- 239000011159 matrix material Substances 0.000 claims description 19
- 239000000126 substance Substances 0.000 claims description 12
- 239000013598 vector Substances 0.000 claims description 11
- 241001061260 Emmelichthys struhsakeri Species 0.000 claims description 9
- 230000001133 acceleration Effects 0.000 claims description 6
- 230000009471 action Effects 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 claims description 3
- 230000008859 change Effects 0.000 claims description 3
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 230000000694 effects Effects 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 238000013178 mathematical model Methods 0.000 claims description 3
- 230000007246 mechanism Effects 0.000 claims description 3
- 230000008569 process Effects 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 3
- 239000005436 troposphere Substances 0.000 claims description 3
- 238000005259 measurement Methods 0.000 claims 2
- 238000011217 control strategy Methods 0.000 claims 1
- 230000008878 coupling Effects 0.000 claims 1
- 238000010168 coupling process Methods 0.000 claims 1
- 238000005859 coupling reaction Methods 0.000 claims 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000004880 explosion Methods 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
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/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/18—Stabilised platforms, e.g. by gyroscope
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Navigation (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
基于GPS和INS组合制导的远程救援机器人系统，包括如下步骤：（1）通过卡尔曼滤波算法进行GPS定位的数据以及陀螺仪、三轴加速度计的数据融合，实现精准定位；（2）根据融合后的定位数据对机器人的运动进行控制；（3）实现人体红外探测和无线电信号发射。本发明所述的方法公开了一种粮基于GPS和INS组合制导的远程救援机器人系统，在丢星无信号的情况下仍能精准提供定向导航服务，并且实时进行人体红外探测，保证了搜救车的救援效率。
Description
技术领域
本发明属于消防器械技术领域，具体涉及到一种基于GPS和INS组合制导的远程救援机器人系统。
背景技术
目前多数的救援机器人采用通过机器人搭载摄像头进行图像实时回传进行控制机器人的运动的，来进行搜索幸存者，这类机器人严重依赖图像数据进行控制机器人，但是无线图像数据传输的距离有所限制，并且会受到现场环境的干扰，导致无法有效控制机器人进行搜救。在GPS和INS组合制导下，可以只提供目标地点坐标，便可根据自主规划的路线进行自动行驶抵达目的地，提高救援效率。
基于GPS和INS组合制导的远程救援机器人系统是基于地震、锅炉爆炸或者大型起重机械倾覆事故，现场环境恶劣，在应急救援过程中极易发生二次危害，搜救人员一般无法进入危险区域进行人员搜救。该救援机器人具有灵活性高、机动性强的特点，可以进入救援人员不易进入的狭窄危险区域进行探索，并且在救援的过程中能够迅速地确定幸存者的位置，消除将救援人员置身在危险境地的状况，减少危险环境中的人员伤亡,提高救援效率与质量。该救援机器人系统运用了卡尔曼滤波算法实现GPS和INS数据进行融合，弥补了单纯GPS导航不够精确、丢星的缺点，运用导航数据实现自我纠正方向，提高了导航的精确性，及时到达指定区域。
发明内容：
本发明旨在发明一种基于GPS和INS组合进行自主制导行驶的远程救援机器人系统。
为解决上述技术问题，本发明提供了如下的技术方案：基于GPS和INS 组合进行自主制导行驶的远程救援机器人系统，依次包括如下步骤：
(1)通过卡尔曼滤波算法进行GPS定位的数据以及陀螺仪、三轴加速度计的数据融合，实现精准定位；
(2)根据融合后的定位数据对机器人的运动进行控制；
(3)实现人体红外探测和无线电信号发射；
1)首先将GPS定位的数据以及陀螺仪、三轴加速度计的数据通过卡尔曼滤波算法进行融合，方法如下：
1、车辆姿态解算方法：
用惯性传感器测量汽车的6自由度状态参数，采用四元数法对车辆的姿态角进行捷联解算，再通过卡尔曼滤波器对姿态角进行最优估计，减小误差。(附图说明，流程图)四元数是一个由四个元构成的数，首先建立系统四元数的微分方程，通过四阶的Runge-Kutta法求解q0，q1，q2，q3，再将其带入车辆姿态矩阵。车辆的四元数姿态矩阵为
其中，q0，q1，q2，q3均为实数标量。
将四元数姿态矩阵与欧拉角姿态矩阵进行对比，可得到姿态角与四元数之间的转换关系。欧拉角姿态矩阵为
其中，
为横摆角、γ为侧倾角、θ为俯仰角，其单位为rad。
可得汽车姿态角为
θ＝arcsin R23
其中，
为横摆角、γ为侧倾角、θ为俯仰角。
2、捷联惯导算法：
导航坐标系下车辆速度的微分方程为
其中，
已经满足可以计算出车辆在导航坐标系下的加速度，所以可以忽略有害成分地向加速度
科里奥利加速度
及重力加速度g。该式表示成三个轴方向上的形式，并对其进行变换后可得结果为
最终可计算出地理坐标系中的经纬度位置为
其中，Rn、Rm为地球半径，L为前纬度，h为高度。
3、差分GPS算法：
根据GPS动态定位法，选择单差定位模型。在多台接收机同时观测的情况下，对于同一颗卫星，可以同时列出多个观测方程，取其中一个观测方程作为基准，其他方程与之相减。基准站的空间坐标为己知，由此解得流动站的空间坐标 [16]。现考虑一个基站一个流动站的动态定位情况，假设a点为基准站，b点为流动站，GPS接收机在两个不同的地点a，b的误差为
当a，b处的接收机选取同一组星座时，ΔSa＝ΔSa＝ΔS，位置差分的定位误差为
其中，右端第一项
当两点相距不远时，
同时因为星钟、星历误差引起的ΔS较小，可以忽略。
则，结果为
位置误差可以分成随机误差V和系统误差B，其中地面多径效应、GPS通道误差、接收机噪声是随机误差的主要原因，对流层在a，b两处分别设置GPS 站点，有
结合上一个公式，可以得到
可以认为，Va、Vb是相互独立的，并且E{V1}＝E{V2}＝0。Ba、Bb有很多相同的元素，令：
由该式可算出P，其中定位误差的RMS值是P的前三个对角线元素之和开方，即：
其中，
通过该式可以看出，由B引起的定位误差只剩下了二阶小量，因此可以认为大部分系统误差被抵消。由V引起的误差增大了，但V引起的误差所占成分较小，综上可以认为差分后定位误差可以大幅度降低。
2)根据融合后的定位数据对机器人的运动进行控制，其控制方法是基于车辆的未来位置和目标位置的偏差值，当车辆行驶的目标路径已知时，利用预测控制的理论得到目标前轮转角，从而实现路径跟踪控制功能。
1、预见点Q以及目标点P
路径跟踪功能的实现要求预见点和给定路径尽量重合。本文中，将点Q定义成预见点，也就是经过时间τ后到达的位置并且车辆始终保持当前的转向角，设xQ＝[xQ，yQ，θQ，αQ]T是预见点的状态向量，可由
求出，其中x，y，θ，α为当前车辆的状态值。
根据求出的Q点的坐标，利用二分法求得目标路径上距离Q点最近的点P的状态量xP＝[xP，yP，θP，αP]T。
2、P点附近的车辆运动方程
预见点以及目标点的状态向量分别为：
对车辆的运动数学模型进行变分获得状态方程为：δx＝A(t)δx+bδx。
上式中δx＝[δx，δy，δθ，δα]T为更新后的状态向量，δu＝δα为更新后的控制向量。A(t)，b分别由下式表示：
b＝[0 0 0 1]T
3、前方转角算法设计
当状态量θp＝0或
时，系统将不能控，所以将速度变化量
看成新的控制量引入以避免不能控的情况，扩张后的车辆运动方程式为δx＝A(t)δx+B(t)u且
根据无限时间调节器的概念设计导航控制器的时候，目标函数为
路径跟踪控制的关键在于x，y，θ的精度跟踪，假设农用车辆为匀速行驶，可以对Q、R做出以下设定，消除行驶速度的影响。
由黎卡提代数方程：
PA+ATP-PBR-1BTP+Q＝0
可以得到的反馈增益矩阵为：
[kvKu]T＝-R-1BTP
由于所求得的kv会很小，并且本文中的控制变量不包括行驶速度，所以在实际情况下的控制器为
δu＝kuδx
将计算出的转角修正量与P点的转向角相加，得到最终转向角如下：
3)实现人体红外探测和无线电信号发射
人体红外探测装置由人体红外探测装置、限位开关、蜗轮蜗杆机构和电动机组成。人体红外探测器装置固定在由蜗轮所构成的平台上，通过蜗轮蜗杆和限位开关的共同作用，可以实现270°的探测扫描范围。人体红外探测仪是通过探测人体发出的热能来感应是否有生命的存在。无线电信号发射装置位于搜救车的前部工作台，其作用是向外界发射信号。当人体红外探测仪感应到被困人员时，会产生1个高电平信号，触发无线电发射装置，使其向外界的救援人员发出信号，以便救援人员确定被困人员的位置，展开救援工作。
通过以上技术方案，本发明的有益效果为：通过搭建的履带式救援机器人原型机，在丢星无信号的情况下仍能精准提供定向导航服务，可以有效的配合Web远程客户端在卫星地图上实时多点监测，路径规划和调控，记录行车数据并自动保存至云端服务器数据库，并且实时进行人体红外探测，保证了搜救车的救援效率。
附图说明：
图1为组合导航系统流程图；
图2为姿态结算路程图；
具体实施方式：
(1)通过卡尔曼滤波算法进行GPS定位的数据以及陀螺仪、三轴加速度计的数据融合，实现精准定位；
(2)根据融合后的定位数据对机器人的运动进行控制；
(3)实现人体红外探测和无线电信号发射；
1)首先将GPS定位的数据以及陀螺仪、三轴加速度计的数据通过卡尔曼滤波算法进行融合，方法如下：
1、车辆姿态解算方法：
用惯性传感器测量汽车的6自由度状态参数，采用四元数法对车辆的姿态角进行捷联解算，再通过卡尔曼滤波器对姿态角进行最优估计，减小误差。(附图说明，流程图)四元数是一个由四个元构成的数，首先建立系统四元数的微分方程，通过四阶的Runge-Kutta法求解q0，q1，q2，q3，再将其带入车辆姿态矩阵。车辆的四元数姿态矩阵为
其中，q0，q1，q2，q3均为实数标量。
将四元数姿态矩阵与欧拉角姿态矩阵进行对比，可得到姿态角与四元数之间的转换关系。欧拉角姿态矩阵为
其中，
为横摆角、γ为侧倾角、θ为俯仰角，其单位为rad。
可得汽车姿态角为
θ＝arcsin R23
其中，
为横摆角、γ为侧倾角、θ为俯仰角。
2、捷联惯导算法：
导航坐标系下车辆速度的微分方程为
其中，
已经满足可以计算出车辆在导航坐标系下的加速度，所以可以忽略有害成分地向加速度
科里奥利加速度
及重力加速度g。该式表示成三个轴方向上的形式，并对其进行变换后可得结果为
最终可计算出地理坐标系中的经纬度位置为
其中，Rn、Rm为地球半径，L为前纬度，h为高度。
3、差分GPS算法：
根据GPS动态定位法，选择单差定位模型。在多台接收机同时观测的情况下，对于同一颗卫星，可以同时列出多个观测方程，取其中一个观测方程作为基准，其他方程与之相减。基准站的空间坐标为己知，由此解得流动站的空间坐标 [16]。现考虑一个基站一个流动站的动态定位情况，假设a点为基准站，b点为流动站，GPS接收机在两个不同的地点a，b的误差为
当a，b处的接收机选取同一组星座时，ΔSa＝ΔSa＝ΔS，位置差分的定位误差为
其中，右端第一项
当两点相距不远时，
同时因为星钟、星历误差引起的ΔS较小，可以忽略。
则，结果为
位置误差可以分成随机误差V和系统误差B，其中地面多径效应、GPS通道误差、接收机噪声是随机误差的主要原因，对流层在a，b两处分别设置GPS 站点，有
结合上一个公式，可以得到
可以认为，Va、Vb是相互独立的，并且E{V1}＝E{V2}＝0。Ba、Bb有很多相同的元素，令：
由该式可算出P，其中定位误差的RMS值是P的前三个对角线元素之和开方，即：
其中，
通过该式可以看出，由B引起的定位误差只剩下了二阶小量，因此可以认为大部分系统误差被抵消。由V引起的误差增大了，但V引起的误差所占成分较小，综上可以认为差分后定位误差可以大幅度降低。
2)根据融合后的定位数据对机器人的运动进行控制，其控制方法是基于车辆的未来位置和目标位置的偏差值，当车辆行驶的目标路径已知时，利用预测控制的理论得到目标前轮转角，从而实现路径跟踪控制功能。
1、预见点Q以及目标点P
路径跟踪功能的实现要求预见点和给定路径尽量重合。本文中，将点Q定义成预见点，也就是经过时间τ后到达的位置并且车辆始终保持当前的转向角，设xQ＝[xQ，yQ，θQ，αQ]T是预见点的状态向量，可由
求出，其中x，y，θ，α为当前车辆的状态值。
根据求出的Q点的坐标，利用二分法求得目标路径上距离Q点最近的点P的状态量xP＝[xP，yP，θP，αP]T。
2、P点附近的车辆运动方程
预见点以及目标点的状态向量分别为：
对车辆的运动数学模型进行变分获得状态方程为：δx＝A(t)δx+bδx。
上式中δx＝[δx，δy，δθ，δα]T为更新后的状态向量，δu＝δα为更新后的控制向量。A(t)，b分别由下式表示：
b＝[0 0 0 1]T
3、前方转角算法设计
当状态量θp＝0或
时，系统将不能控，所以将速度变化量
看成新的控制量引入以避免不能控的情况，扩张后的车辆运动方程式为δx＝A(t)δx+B(t)u，且
根据无限时间调节器的概念设计导航控制器的时候，目标函数为
路径跟踪控制的关键在于x，y，θ的精度跟踪，假设农用车辆为匀速行驶，可以对Q、R做出以下设定，消除行驶速度的影响。
由黎卡提代数方程：
PA+ATP-PBR-1BTP+Q＝0
可以得到的反馈增益矩阵为：
[kvKu]T＝-R-1BTP
由于所求得的kv会很小，并且本文中的控制变量不包括行驶速度，所以在实际情况下的控制器为
δu＝kuδx
将计算出的转角修正量与P点的转向角相加，得到最终转向角如下：
3)实现人体红外探测和无线电信号发射
人体红外探测装置由人体红外探测装置、限位开关、蜗轮蜗杆机构和电动机组成。人体红外探测器装置固定在由蜗轮所构成的平台上，通过蜗轮蜗杆和限位开关的共同作用，可以实现270°的探测扫描范围。人体红外探测仪是通过探测人体发出的热能来感应是否有生命的存在。无线电信号发射装置位于搜救车的前部工作台，其作用是向外界发射信号。当人体红外探测仪感应到被困人员时，会产生1个高电平信号，触发无线电发射装置，使其向外界的救援人员发出信号，以便救援人员确定被困人员的位置，展开救援工作。
对该系统搭建的履带式救援机器人原型机，在丢星无信号的情况下仍能精准提供定向导航服务，并且实时进行人体红外探测，保证了搜救的救援效率。
Claims (3)
1.一种基于GPS和INS组合制导的远程救援机器人系统，其特征是：
(1)通过卡尔曼滤波算法进行GPS定位的数据以及陀螺仪、三轴加速度计的数据融合，实现精准定位；
(2)根据融合后的定位数据对机器人的运动进行控制；
(3)实现人体红外探测和无线电信号发射；
1)首先将GPS定位的数据以及陀螺仪、三轴加速度计的数据通过卡尔曼滤波算法进行融合，方法如下：
①车辆姿态解算方法：
用惯性传感器测量汽车的6自由度状态参数，采用四元数法对车辆的姿态角进行捷联解算，再通过卡尔曼滤波器对姿态角进行最优估计，减小误差；(附图说明，流程图)四元数是一个由四个元构成的数，首先建立系统四元数的微分方程，通过四阶的Runge-Kutta法求解q0，q1，q2，q3，再将其带入车辆姿态矩阵；车辆的四元数姿态矩阵为
其中，q0，q1，q2，q3均为实数标量；
将四元数姿态矩阵与欧拉角姿态矩阵进行对比，可得到姿态角与四元数之间的转换关系；欧拉角姿态矩阵为
其中，
为横摆角、γ为侧倾角、θ为俯仰角，其单位为rad；
可得汽车姿态角为
θ＝arcsin R23
其中，
为横摆角、γ为侧倾角、θ为俯仰角；
②捷联惯导算法：
导航坐标系下车辆速度的微分方程为
其中，
已经满足可以计算出车辆在导航坐标系下的加速度，所以可以忽略有害成分地向加速度
科里奥利加速度
及重力加速度g；该式表示成三个轴方向上的形式，并对其进行变换后可得结果为
最终可计算出地理坐标系中的经纬度位置为
其中，Rn、Rm为地球半径，L为前纬度，h为高度；
③差分GPS算法：
根据GPS动态定位法，选择单差定位模型；在多台接收机同时观测的情况下，对于同一颗卫星，可以同时列出多个观测方程，取其中一个观测方程作为基准，其他方程与之相减；基准站的空间坐标为己知，由此解得流动站的空间坐标[16]；现考虑一个基站一个流动站的动态定位情况，假设a点为基准站，b点为流动站，GPS接收机在两个不同的地点a，b的误差为
当a，b处的接收机选取同一组星座时，ΔSa＝ΔSa＝ΔS，位置差分的定位误差为
其中，右端第一项
当两点相距不远时，
同时因为星钟、星历误差引起的ΔS较小，可以忽略；则，结果为
位置误差可以分成随机误差V和系统误差B，其中地面多径效应、GPS通道误差、接收机噪声是随机误差的主要原因，对流层在a，b两处分别设置GPS站点，有
结合上一个公式，可以得到
可以认为，Va、Vb是相互独立的，并且E{V1}＝E{V2}＝0；Ba、Bb有很多相同的元素，令：
由该式可算出P，其中定位误差的RMS值是P的前三个对角线元素之和开方，即：
其中，
通过该式可以看出，由B引起的定位误差只剩下了二阶小量，因此可以认为大部分系统误差被抵消；由V引起的误差增大了，但V引起的误差所占成分较小，综上可以认为差分后定位误差可以大幅度降低；
2)根据融合后的定位数据对机器人的运动进行控制，其控制方法是基于车辆的未来位置和目标位置的偏差值，当车辆行驶的目标路径已知时，利用预测控制的理论得到目标前轮转角，从而实现路径跟踪控制功能；
①预见点Q以及目标点P
路径跟踪功能的实现要求预见点和给定路径尽量重合；本文中，将点Q定义成预见点，也就是经过时间τ后到达的位置并且车辆始终保持当前的转向角，设xQ＝[xQ，yQ，θQ，αQ]T是预见点的状态向量，可由
求出，其中x，y，θ，α为当前车辆的状态值；
根据求出的Q点的坐标，利用二分法求得目标路径上距离Q点最近的点P的状态量xP＝[xP，yP，θP，αP]T；
②P点附近的车辆运动方程
预见点以及目标点的状态向量分别为：
对车辆的运动数学模型进行变分获得状态方程为：δx＝A(t)δx+bδx；
上式中δx＝[δx，δy，δθ，δα]T为更新后的状态向量，δu＝δα为更新后的控制向量；A(t)，b分别由下式表示：
b＝[0 0 0 1]1
③前方转角算法设计
当状态量θp＝0或
时，系统将不能控，所以将速度变化量
看成新的控制量引入以避免不能控的情况，扩张后的车辆运动方程式为δx＝A(t)δx+B(t)u且
根据无限时间调节器的概念设计导航控制器的时候，目标函数为
路径跟踪控制的关键在于x，y，θ的精度跟踪，假设农用车辆为匀速行驶，可以对Q、R做出以下设定，消除行驶速度的影响；
由黎卡提代数方程：
PA+ATP-PBR-1BTP+Q＝0
可以得到的反馈增益矩阵为：
[kvKu]T＝-R-1BTP
由于所求得的kv会很小，并且本文中的控制变量不包括行驶速度，所以在实际情况下的控制器为
δu＝kuδx
将计算出的转角修正量与P点的转向角相加，得到最终转向角如下：
3)实现人体红外探测和无线电信号发射
人体红外探测装置由人体红外探测装置、限位开关、蜗轮蜗杆机构和电动机组成；人体红外探测器装置固定在由蜗轮所构成的平台上，通过蜗轮蜗杆和限位开关的共同作用，可以实现270°的探测扫描范围；人体红外探测仪是通过探测人体发出的热能来感应是否有生命的存在；无线电信号发射装置位于搜救车的前部工作台，其作用是向外界发射信号；当人体红外探测仪感应到被困人员时，会产生1个高电平信号，触发无线电发射装置，使其向外界的救援人员发出信号，以便救援人员确定被困人员的位置，展开救援工作。
2.如权利1要求所述的组合导航定位系统有GPS接收机模块、INS模块和组合导航计算模块，三个模块采用松耦合方式，将GPS和INS各自输出的位置估值和速度估值进行比较，得到的差值作为卡尔曼滤波器的测量输入值，对惯导系统提供测量更新，以提高惯性导航的精度。
3.如权利1所述整个自动行驶过程由组合导航算法、路径跟踪控制及转向执行电机控制策略。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010603813.5A CN111722257A (zh) | 2020-06-29 | 2020-06-29 | 一种基于gps和ins组合制导的远程救援机器人系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010603813.5A CN111722257A (zh) | 2020-06-29 | 2020-06-29 | 一种基于gps和ins组合制导的远程救援机器人系统 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN111722257A true CN111722257A (zh) | 2020-09-29 |
Family
ID=72569488
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010603813.5A Pending CN111722257A (zh) | 2020-06-29 | 2020-06-29 | 一种基于gps和ins组合制导的远程救援机器人系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111722257A (zh) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112977603A (zh) * | 2021-02-20 | 2021-06-18 | 黑龙江惠达科技发展有限公司 | 一种电机控制的方法和装置 |
| CN113091772A (zh) * | 2021-05-07 | 2021-07-09 | 成都耀塔科技有限公司 | 一种室内导航传感器系统误差校正方法 |
| CN114237252A (zh) * | 2021-12-17 | 2022-03-25 | 河南工业大学 | 一种融合卡尔曼滤波预测思想的适用于移动机器人导航的改进蚁群算法 |
| CN114463932A (zh) * | 2022-01-14 | 2022-05-10 | 国网江苏省电力工程咨询有限公司 | 非接触式施工安全距离主动动态识别预警系统及方法 |
| CN116027371A (zh) * | 2023-03-27 | 2023-04-28 | 山东科技大学 | 一种海上救援示位终端定位数据处理方法 |
| CN116839591A (zh) * | 2023-07-12 | 2023-10-03 | 哈尔滨天枢问道技术有限公司 | 一种轨迹跟踪定位滤波系统及救援无人机的融合导航方法 |
-
2020
- 2020-06-29 CN CN202010603813.5A patent/CN111722257A/zh active Pending
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112977603A (zh) * | 2021-02-20 | 2021-06-18 | 黑龙江惠达科技发展有限公司 | 一种电机控制的方法和装置 |
| CN112977603B (zh) * | 2021-02-20 | 2022-05-06 | 黑龙江惠达科技发展有限公司 | 电机控制的方法和装置、农机以及计算机可读存储介质 |
| CN113091772A (zh) * | 2021-05-07 | 2021-07-09 | 成都耀塔科技有限公司 | 一种室内导航传感器系统误差校正方法 |
| CN114237252A (zh) * | 2021-12-17 | 2022-03-25 | 河南工业大学 | 一种融合卡尔曼滤波预测思想的适用于移动机器人导航的改进蚁群算法 |
| CN114237252B (zh) * | 2021-12-17 | 2023-05-26 | 河南工业大学 | 一种融合卡尔曼滤波预测思想的适用于移动机器人导航的改进蚁群算法 |
| CN114463932A (zh) * | 2022-01-14 | 2022-05-10 | 国网江苏省电力工程咨询有限公司 | 非接触式施工安全距离主动动态识别预警系统及方法 |
| CN114463932B (zh) * | 2022-01-14 | 2024-05-03 | 国网江苏省电力工程咨询有限公司 | 非接触式施工安全距离主动动态识别预警系统及方法 |
| CN116027371A (zh) * | 2023-03-27 | 2023-04-28 | 山东科技大学 | 一种海上救援示位终端定位数据处理方法 |
| CN116839591A (zh) * | 2023-07-12 | 2023-10-03 | 哈尔滨天枢问道技术有限公司 | 一种轨迹跟踪定位滤波系统及救援无人机的融合导航方法 |
| CN116839591B (zh) * | 2023-07-12 | 2024-05-28 | 哈尔滨天枢问道技术有限公司 | 一种轨迹跟踪定位滤波系统及救援无人机的融合导航方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111722257A (zh) | 一种基于gps和ins组合制导的远程救援机器人系统 | |
| García Carrillo et al. | Combining stereo vision and inertial navigation system for a quad-rotor UAV | |
| US9122278B2 (en) | Vehicle navigation | |
| Balaram | Kinematic state estimation for a Mars rover | |
| JPH095104A (ja) | 移動物体の三次元姿勢角測定法および三次元姿勢角計測装置 | |
| Balaram | Kinematic observers for articulated rovers | |
| Tuvshinjargal et al. | Hybrid motion planning method for autonomous robots using kinect based sensor fusion and virtual plane approach in dynamic environments | |
| Gifford et al. | Low-cost multi-robot exploration and mapping | |
| Baumgartner et al. | State estimation and vehicle localization for the FIDO rover | |
| Maeda et al. | Flight path planning of multiple UAVs for robust localization near infrastructure facilities | |
| JP6949417B1 (ja) | 車両操縦システムと車両操縦方法 | |
| Caltabiano et al. | A robotic system for volcano exploration | |
| Narkhede et al. | Inertial sensors and GPS fusion using LSTM for position estimation of aerial vehicle | |
| CN111221053B (zh) | 一种自动搜救定位系统、装置及定位方法 | |
| Ray et al. | GPS and sonar based area mapping and navigation by mobile robots | |
| CN211012986U (zh) | 一种基于惯导技术的无人自主巡航车导航系统 | |
| Huang et al. | Inertial Sensor Error Compensation for Global Positioning System Signal Blocking--Extended Kalman Filter vs Long-and Short-term Memory--. | |
| Murphy et al. | GPS-aided retrotraverse for unmanned ground vehicles | |
| Wang | A driverless vehicle vision path planning algorithm for sensor fusion | |
| Shaukat et al. | UGV Localization With AI-Assisted EKF for Multi-Terrain Environments | |
| Webster | A localization solution for an autonomous vehicle in an urban environment | |
| Thai et al. | Autonomous Mobile Robot: Navigating and Monitoring Fire Safety at Power Substations | |
| Sridhar et al. | Location Determination of an Unmanned Aerial Vehicle in a GPS-Denied, Hazard-Cluttered Indoor Environment | |
| Dlouhy et al. | Applications of autonomous navigation in next-generation mars rovers | |
| Hsiao et al. | The development of a target-lock-on optical remote sensing system for unmanned aerial vehicles |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| WD01 | Invention patent application deemed withdrawn after publication | ||
| WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20200929 |