我在ROS中应用比例导数控制器来控制我的模型。我仅限于python 2.7.17版本。
这个脚本中有两种类型的错误;位置误差(ep)和航向误差(eth)。
我给了last_error=0，并试图得到更新(ep_dot)和(eth_dot)作为一种方法来找到误差的导数。我想知道我给出的导数公式(ep_dot)和(eth_dot)是否正确。这是求导数的正确方法吗?还有其他相关的方法可以找到相同的吗?请告诉我解决办法。
#!/usr/bin/env python
import rospy
import math
import csv
import time
from time import gmtime,strftime
from datetime import datetime
import numpy as np # for converting radians to degrees
from geometry_msgs.msg import Twist #to obtain command velocities
from nav_msgs.msg import Odometry #to obtain position and orientation
from tf.transformations import euler_from_quaternion, quaternion_from_euler
roll = pitch = yaw = 0.0
t_start = time.time()
**last_error =0**
def get_rotation(msg):
velo_msg = Twist()
global roll,pitch,yaw
orientation_q = msg.pose.pose.orientation
orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w]
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
kpp = 0.74
kpth = 0.5
kd = 0.1
Tmax = 60
t_milli = (time.time() - t_start)*1000
t = t_milli/1000 # to get the values in seconds
print("t=",t)
if t > Tmax:
rospy.signal_shutdown("Simulation ends here!")
x_now = msg.pose.pose.position.x
y_now = msg.pose.pose.position.y
th = yaw
xT = math.cos(2*math.pi*t/Tmax)*0.8
yT = math.sin(2*math.pi*t/Tmax)*0.8
#Trasformation matrix # Finding INVERSE
I = np.array([[math.cos(th), -math.sin(th), x_now],[math.sin(th), math.cos(th), y_now],[0, 0, 1]])
B = np.array([[xT],[yT],[1]]) # print('B=',B)
C = np.dot(np.linalg.inv(I),B) # np.dot: for matrix multiplication
xTB = C[0][0] # [] indexing to extract the values of an array/matrix
yTB = C[1][0]
ep = math.sqrt(xTB*xTB + yTB*yTB) # error calc.
ep_dot = (ep-last_error)/t
eth = math.atan2(yTB,xTB)
eth_dot = (eth-last_error)/t
print('ep =',ep,'eth(deg)=',eth*180/math.pi,'radius=',math.sqrt(x_now*x_now + y_now*y_now),'t=',t)
PID_lin = ep*kpp + ep_dot*kd
PID_ang = eth*kpth - eth_dot*kd
# publishing the cmd_vel in linear and angular motion both
velo_msg.linear.x = PID_lin
velo_msg.angular.z = PID_ang
pub.publish(velo_msg)
rospy.init_node('shadan')
sub = rospy.Subscriber("/odom", Odometry, get_rotation)
pub = rospy.Publisher('/cmd_vel',Twist,queue_size=10)
r = rospy.Rate(10)
while not rospy.is_shutdown():
r.sleep()
我认为你对导数项的计算有问题。
你得到t_start=time.time()在你的代码的最开始，然后每次你得到你的回调你更新t_milli = (time.time() - t_start)*1000和t = t_milli/1000与t_start是一个常数。
则计算ep_dot等于ep_dot = (ep-last_error)/t。但是ep_dot应该等于ep_dot = (actual_value - last_value)/(t_actual - t_last_value)
导数项应该等于两个连续值的差除以这两个值之间的时间差，所以你需要将回调中获得的前一个数据的时间存储在一个新的变量中。
同样，比例误差的计算应该等于您想要的值(目标)减去实际值。然后乘以P系数
希望它能解决你的问题!