python - 在 GPS 不可用的情况下从 IMU 数据计算位置
问题描述
第一次在这里发帖，我双脚跳进蟒蛇。
我的项目是尝试仅使用 IMU 传感器和速度表来计算水下机器人的位置。
我对编程很陌生，我相信我会在附加的代码上得到很多很好的反馈，但我目前坚持的步骤是在以下之间创建一个反馈循环：
UTC[2]（GPS 状态 A=可用 V=不可用），
LATD/ LOND（十进制度的GPS位置），和
IMU_LAT/ IMU_LON（以十进制度为单位的 IMU 位置）
这个想法是，如果UTC[2]是“A”，逻辑将同样平均IMU_LAT/IMU_LON和LATD/ LOND，但如果UTC[2]是“V”，它只会根据记录的最后位置和IMU_north/ IMU_east（基于航向和加速度值的偏移量）计算位置。
import time, inspect
import IMU
import serial
import datetime
import os
import math
import logging
log = open(time.strftime("%Y%m%d-%H%M%S")+'_GSPData.csv','w')
#f.write("UTC TIME,NAVSTATUS,LAT,LON,HDG,SPD,X,Y,Z")
RAD_TO_DEG = 57.29578
M_PI = 3.14159265358979323846
G_GAIN = 0.070 # [deg/s/LSB] If you change the dps for gyro, you need to update this value accordingly
AA = 0.40 # Complementary filter constant
magXmin = 0
magYmin = 0
magZmin = 0
magXmax = 0
magYmax = 0
magZmax = 0
gyroXangle = 0.0
gyroYangle = 0.0
gyroZangle = 0.0
CFangleX = 0.0
CFangleY = 0.0
IMU.detectIMU() #Detect if BerryIMUv1 or BerryIMUv2 is connected.
IMU.initIMU() #Initialise the accelerometer, gyroscope and compass
a = datetime.datetime.now()
ser = serial.Serial('/dev/serial0', 9600)
def truncate(n, decimals=0):
multiplier = 10 ** decimals
return int(n * multiplier) / multiplier
log.write("UTC,NAVSTAT,LAT,LON,HDG,SPD,xm/s,ym/s,zm/s")
log.write("\n")
try:
while True:
#Read the GPS, accelerometer, gyroscope and magnetometer values
NMEA = ser.readline()
NMEA_str_data = NMEA.decode('utf-8')
NMEA_data_arr=NMEA_str_data.split()
UTC = NMEA_str_data.split(',')
GYRx = IMU.readGYRx()
GYRy = IMU.readGYRy()
GYRz = IMU.readGYRz()
ACCx = IMU.readACCx()
ACCy = IMU.readACCy()
ACCz = IMU.readACCz()
#output the values of the accelerometer in m/s
yG = ((ACCx * 0.244)/1000)*9.80665
xG = ((ACCy * 0.244)/1000)*9.80665
zG = ((ACCz * 0.244)/1000)*9.80665
MAGx = IMU.readMAGx()
MAGy = IMU.readMAGy()
MAGz = IMU.readMAGz()
#Apply compass calibration
MAGx -= (magXmin + magXmax) /2
MAGy -= (magYmin + magYmax) /2
MAGz -= (magZmin + magZmax) /2
##Calculate loop Period(LP). How long between Gyro Reads
b = datetime.datetime.now() - a
a = datetime.datetime.now()
LP = b.microseconds/(1000000*1.0)
outputString = "Loop Time %5.2f " % ( LP )
#Convert Gyro raw to degrees per second
rate_gyr_x = GYRx * G_GAIN
rate_gyr_y = GYRy * G_GAIN
rate_gyr_z = GYRz * G_GAIN
#Calculate the angles from the gyro.
gyroXangle+=rate_gyr_x*LP
gyroYangle+=rate_gyr_y*LP
gyroZangle+=rate_gyr_z*LP
#Convert Accelerometer values to degrees
AccXangle = (math.atan2(ACCy,ACCz)*RAD_TO_DEG)
AccYangle = (math.atan2(ACCz,ACCx)+M_PI)*RAD_TO_DEG
#convert the values to -180 and +180
if AccYangle > 90:
AccYangle -= 270.0
else:
AccYangle += 90.0
#Complementary filter used to combine the accelerometer and gyro values.
CFangleX=AA*(CFangleX+rate_gyr_x*LP) +(1 - AA) * AccXangle
CFangleY=AA*(CFangleY+rate_gyr_y*LP) +(1 - AA) * AccYangle
#Calculate heading
heading = 180 * math.atan2(MAGy,MAGx)/M_PI
#Only have our heading between 0 and 360
if heading < 0:
heading += 360
####################################################################
###################Tilt compensated heading#########################
####################################################################
#Normalize accelerometer raw values.
accXnorm = ACCx/math.sqrt(ACCx * ACCx + ACCy * ACCy + ACCz * ACCz)
accYnorm = ACCy/math.sqrt(ACCx * ACCx + ACCy * ACCy + ACCz * ACCz)
accZnorm = ACCz/math.sqrt(ACCx * ACCx + ACCy * ACCy + ACCz * ACCz)
Zms_norm = zG-9.80665
Yms_norm = yG
Xms_norm = xG
#Calculate course
Course = (180*math.atan2(Xms_norm,Yms_norm)/M_PI)
#Only have our course between 0 and 360
if Course < 0:
Course +=360
#Calculate pitch and roll
pitch = math.asin(accXnorm)
roll = -math.asin(accYnorm/math.cos(pitch))
#Calculate the new tilt compensated values
magXcomp = MAGx*math.cos(pitch)+MAGz*math.sin(pitch)
#The compass and accelerometer are orientated differently on the LSM9DS0 and LSM9DS1 and the Z axis on the compass
#is also reversed. This needs to be taken into consideration when performing the calculations
if(IMU.LSM9DS0):
magYcomp = MAGx*math.sin(roll)*math.sin(pitch)+MAGy*math.cos(roll)-MAGz*math.sin(roll)*math.cos(pitch) #LSM9DS0
else:
magYcomp = MAGx*math.sin(roll)*math.sin(pitch)+MAGy*math.cos(roll)+MAGz*math.sin(roll)*math.cos(pitch) #LSM9DS1
#Calculate tilt compensated heading
tiltCompensatedHeading = 180 * math.atan2(magYcomp,magXcomp)/M_PI
if tiltCompensatedHeading < 0:
tiltCompensatedHeading += 360
#convert IMU readings to northings and eastings
IMU_north= (math.cos(tiltCompensatedHeading))*(Yms_norm+Xms_norm)
IMU_east= (math.sin(tiltCompensatedHeading))*(Yms_norm+Xms_norm)
#convert IMU_north to D.D
IMU_north_D= IMU_north/110723.41272
#Convert IMU_east to d.d
IMU_east_D= IMU_east/103616.02936
############################ END ##################################
#"%am/s": no rounding "%bm/s": unsupported "%cm/s": unsupported
#"%dm/s": whole numbers "%em/s": scientific notation "%fm/s": six digits
#"%gm/s": five digits
if NMEA_str_data.startswith('$GNRMC'):
if UTC[2] =="V":
#print("GPS unavaliable","heading",round(tiltCompensatedHeading,2),",course",round(Course,2),xG,yG,zG,"IMU_LAT","IMU_LON")
print("UTC",truncate(float(UTC[1]),0),",IMU",",LAT",",LON",",heading",round(tiltCompensatedHeading,2),",course",round(Course,2),truncate(IMU_north,4),truncate(IMU_east,4))
#log the output GPS invalid
log.write(UTC[1]+','+UTC[2]+','+""+','""+','+str(round(tiltCompensatedHeading,2))+','+UTC[7]+','+str(IMU_north)+','+str(IMU_east))
else:
#convert UTC from DDMM.MMM to DD.DDDD
if UTC[4] =="N":
LATD= (truncate(float(UTC[3]),-2)/100)+((float(UTC[3])-(truncate(float(UTC[3]),-2)))/60)
else:
LATD= -(truncate(float(UTC[3]),-2)/100)+((float(UTC[3])-(truncate(float(UTC[3]),-2)))/60)
if UTC[6] =="E":
LOND= (truncate(float(UTC[5]),-2)/100)+((float(UTC[5])-(truncate(float(UTC[5]),-2)))/60)
else:
LOND= -(truncate(float(UTC[5]),-2)/100)+((float(UTC[5])-(truncate(float(UTC[5]),-2)))/60)
#calculate IMU_LAT
IMU_LAT= LATD+IMU_north_D
#Calculate IMU_LON
IMU_LON= LOND+IMU_east_D
#write the output
print("UTC",truncate(float(UTC[1]),0),",GPS",",LAT",truncate(LATD,5),truncate(IMU_LAT,5),",LON",truncate(LOND,5),truncate(IMU_LON,5),",heading",round(tiltCompensatedHeading,2),",course",round(Course,2),UTC[8],",speed",truncate(float(UTC[7]),2))
#log the output GPS valid
log.write(UTC[1]+','+UTC[2]+','+str(LATD)+','+str(LOND)+','+str(round(tiltCompensatedHeading,2))+','+UTC[7]+','+str(IMU_north)+','+str(IMU_east))
log.write("\n")
#slow program down a bit, makes the output more readable
time.sleep(0.5)
#print(" aX = %fG aY =%fG aZ =%fG " % ( ACCx, ACCy, ACCz))
#slow program down a bit, makes the output more readable
#time.sleep(0.5)
except (KeyboardInterrupt, SystemExit): #when you press ctrl+c
print ("Done.\nExiting.")
log.close()
就像我说的那样，我是新手，我相信你们专业人士会告诉我这真的很草率，但我很乐意接受任何建设性的批评。
谢谢，特洛伊
解决方案
推荐阅读
- spring - Intellij MavenWrapperDownloader.java：无法解析符号错误
- graylog - JSON提取器和嵌套字段的问题
- javascript - 尝试嵌套 if else 语句以进行表单验证
- mysql - 在事件日志 AWS RDS MySQL 中找不到 MyISAM 表
- java - 为什么级联持久化不适用于 Hibernate 中的多对多关系？
- jmeter - JMeter CLI unix 脚本说明
- c++ - SFINAE：“enable_if”不能用于禁用此声明
- reactjs - source.uri 不应为空字符串
- python - 选择字典中的项目
- javascript - 给定值范围内的百分比