问题描述
我的项目是尝试仅使用IMU传感器和速度表来计算水下机器人的位置。
我对编程很陌生，我敢肯定我会在所附的代码中获得很多反馈，但是我目前坚持的步骤是在以下之间创建一个反馈循环：
LATD（GPS A的状态=可用V =不可用），
LOND / IMU_LAT（十进制GPS位置）和
IMU_LON / UTC[2]（IMU位置以十进制表示）
该想法是，如果IMU_LAT为“ A”，则逻辑将平均IMU_LON / LATD和LOND / UTC[2]平均，但如果{{ 1}}为“ 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,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,xG,yG,zG,"IMU_LAT","IMU_LON")
print("UTC",truncate(float(UTC[1]),0),IMU",LAT",LON",heading",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)))/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)))/60)
#calculate IMU_LAT
IMU_LAT= LATD+IMU_north_D
#Calculate IMU_LON
IMU_LON= LOND+IMU_east_D
#write the output
print("UTC",GPS",truncate(LATD,5),truncate(IMU_LAT,truncate(LOND,truncate(IMU_LON,UTC[8],speed",truncate(float(UTC[7]),2))
#log the output GPS valid
log.write(UTC[1]+','+str(LATD)+','+str(LOND)+','+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()
就像我说的我是新手一样，我相信你们的专业人士会告诉我它的确很草率，但是我很乐意接受任何建设性的批评。
谢谢特洛伊
解决方法
暂无找到可以解决该程序问题的有效方法，小编努力寻找整理中！
如果你已经找到好的解决方法，欢迎将解决方案带上本链接一起发送给小编。
小编邮箱:dio#foxmail.com (将#修改为@）