基于waypoint的路径规划与控制算法的实现（下）
2022-12-22 # 实用教程 #Autonomous Driving #Motion Planning #ROS #Python

关于本文

参考上一篇文章————基于waypoint的路径规划与控制算法的实现（上）

路径跟随

WayPointFollower程序主要完成了这些工作，加载路径点，路径可视化，获取当前位姿，计算发布控制指令。

流程图

下面是轨迹跟随算法的流程图。
WayPointFollower

补充一点，加载路径点后面少了可视化部分，因为我最开始没打算写的，后来为了调试方便才加的。

难点

路径跟随最难处理的地方在于转角控制的计算了，首先来看下实际循迹过程会出现的两种情况。
下面是第一种情况，也是比较理想的情况，根据当前位置获取到的最近路径点刚好在前面。

情况1

这时我们根据当前位置前后两个路径点的yaw角偏移就能计算出当前控制转角的大小，如公式(1.1)所示：

$$(Y_n-Y_c)\times\alpha+(Y_n-Y_b)\times\beta \tag{1.1}$$

其中Y_n、Y_c、Y_b分别为当前yaw角，最近路径点yaw角跟后一个路径点的yaw角，$\alpha$ 跟 $\beta$ 为不同的固定权重且满足等式(1.2)

$$\alpha+\beta=1 \tag{1.2}$$

另一种情况如下所示，根据当前位置获取到的最近路径点在后面了，这时再根据最近点和它后一个路径点来计算会发生较大的误差。

情况2

理想的算法是找到最近的两个路径点，然后根据它们相对于当前位置的距离来分配权重大小（距离近权重大），在把权重放进控制转角计算中，如公式(2.1)所示：
$$(Y_n-Y_{c1})\times\alpha+(Y_n-Y_{c2})\times\beta \tag{2.1}$$

其中$Y_{c1}$、$Y_{c2}$分别为最近路径点yaw的角跟第二近的路径点yaw角。

需要注意的是$\alpha$ 跟 $\beta$在这里是根据距离比重浮动变化的，参考式(2.2)跟式(2.3)：

$$\alpha=d_1 \div (d_1+d_2) \tag{2.2}$$

其中 $d_1$ 为当前位置与最近路径点的距离， $d_2$ 为当前位置与第二近的路径点距离。

$$\beta=d_2 \div (d_1+d_2) \tag{2.3}$$

但实际上路径点并没有离的那么远，别忘了我们采样频率是2hz，这个频率在低速情况下采集的路径点是非常密的，所以只要权重分配到位，第二种情况是可以跟第一种情况近似处理。

主程序

跟waypoint_saver一样，WayPointFollower的程序入口也在最底下if __name__ == "__main__":那里，主循环跑的是WayPointFollower()对象的run()函数。注释应该写的很详细了，这里就不再赘述了。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
import math
import rospy   
import tf    
from tf_conversions import transformations  
import csv
from geometry_msgs.msg import Twist
from visualization_msgs.msg import Marker
from visualization_msgs.msg import MarkerArray

class WayPointFollower:
    def __init__(self):
        self._seq = 0   # use to figure out visiualized waypoint
        self._wpIdx = 0
        self._goalIdx = 0
        self._wps = []
        self._cur_pose=[]
        self._filename = 'waypoints.csv'
        self._stop_flag = False
        self.v = Twist()
        self.tf_listener = tf.TransformListener()
        self.markerArray = MarkerArray()
        self.cmd_pub = rospy.Publisher('cmd_vel',Twist,queue_size=10)
        self.rviz_pub = rospy.Publisher('waypoints', MarkerArray, queue_size=10)

    def _load_waypoints(self):
        ''' Load waypoints to a list '''
        with open(self._filename,'r') as f:
            rdr = csv.reader(f)
            for i,row in enumerate(rdr):
                if i>0:     # remove header
                    # convert str to float
                    wp = []
                    for p in row:
                        wp.append(float(p))
                    self._wps.append(wp)
        # record final waypoint index
        self._goalIdx = len(self._wps)-1

    # def _preprocess_wps(self):
    #     ''' not used yet '''
    #     offset = 7
    #     for i in self._wps:
    #         if offset >= self._goalIdx:
    #             break
    #         max = 0
    #         idx = 0
    #         print(len(self._wps[i:offset]))
    #         for wp in self._wps[:offset]:
    #             if wp[-1] > max:
    #                 max = wp[-1]
    #                 idx = wp[0]
    #         print(max,idx)
    #         offset+=1

    def _visualize_wps(self):
        ''' Visualize waypoints in rviz '''
        ourmarkerArray = []
        for wp in self._wps:
            marker = Marker()
            marker.header.frame_id = "/map"
            marker.header.stamp = rospy.Time.now()
            marker.header.seq = self._seq
            marker.type = marker.SPHERE
            marker.action = marker.ADD
            marker.id = wp[0]
            marker.scale.x = 0.2
            marker.scale.y = 0.2
            marker.scale.z = 0.2
            marker.color.a = 1.0
            marker.color.r = 1.0
            marker.color.g = 1.0
            marker.color.b = 0.0
            marker.pose.orientation.w = 1.0
            marker.pose.position.x = wp[1]
            marker.pose.position.y = wp[2]
            marker.pose.position.z = 0
            
            ourmarkerArray.append(marker)
        self._seq += 1
        self.markerArray = ourmarkerArray
        self.rviz_pub.publish(self.markerArray)

    def _get_current_pos(self):
        ''' Get current position '''
        try:
            trans, rot = self.tf_listener.lookupTransform('/map', '/base_link', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            return False
        roll,pitch,yaw = transformations.euler_from_quaternion(rot)
        x = trans[0]
        y = trans[1]
        self._cur_pose = [x,y,yaw]
        if len(self._cur_pose):
            return True
        else:
            return False

    def _get_closet_wpIdx(self): 
        ''' get closest waypoint from remaining waypoints '''  
        cur_wpIdx = self._wpIdx
        if self._get_current_pos(): 
            min = math.sqrt(math.pow(self._cur_pose[0]-self._wps[0][1],2)+math.pow(self._cur_pose[1]-self._wps[0][2],2))
            for wp in self._wps[cur_wpIdx:-1]:
                dst = math.sqrt(math.pow(self._cur_pose[0]-wp[1],2)+math.pow(self._cur_pose[1]-wp[2],2))
                if dst < min:
                    min = dst
                    self._wpIdx = int(wp[0])

    def _reach_the_goal(self):
        ''' Check if reach the goal '''
        if self._wpIdx+1 == self._goalIdx:
            self._stop_flag = True
            return True
        return False
        
    def _send_vel_cmd(self):
        ''' Compute and send velocity command '''
        self.v.linear.x = 0.1
        prev_wp_angle = self._wps[self._wpIdx + 1][-1] - self._cur_pose[-1]
        back_wp_angle = self._wps[self._wpIdx][-1] - self._cur_pose[-1]
        self.v.angular.z = (prev_wp_angle * 4 + back_wp_angle * 6)/10
        if self.v.angular.z > 0.5:
            self.v.angular.z = 0.8 * self.v.angular.z
        self.cmd_pub.publish(self.v)

    def _stop(self):
        ''' Send stop command '''
        self.v.linear.x = 0
        self.v.angular.z = 0
        while self._stop_flag:
            self.cmd_pub.publish(self.v)

    def run(self):
        self._load_waypoints()
        # self._preprocess_wps()
        r = rospy.Rate(2)
        while not rospy.is_shutdown():
            self._visualize_wps()
            if self._reach_the_goal():
                self._stop()
            if self._get_current_pos():
                self._get_closet_wpIdx()
                self._send_vel_cmd()
            r.sleep()

if __name__ == "__main__":
    rospy.init_node('wayPoint_follower')   
    follower = WayPointFollower()  
    follower.run()
上一页
2022-12-22 13:55:43 # 实用教程 #Autonomous Driving #Motion Planning #ROS #Python
下一页