Trước đây, chúng ta đã xem xét bản đồ tĩnh và bản đồ chướng ngại vật, nhưng đối với lập kế hoạch đường đi, hai bản đồ này là không đủ, chúng ta cần thêm một bản đồ thứ ba, đó là bản đồ phình to.
Bản đồ phình to khá dễ hiểu, đó là việc phình to các chướng ngại vật trên bản đồ. Tại sao phải thực hiện thao tác này? Chủ yếu là vì khi lập kế hoạch đường đi, đối với bản đồ tĩnh, việc lập kế hoạch đường đi không dễ dàng, vì một ô lưới có thể trống nhưng nếu nó ở gần chướng ngại vật thì thực tế robot không thể đến được điểm đó. Do đó, chúng ta cần phình to bản đồ theo bán kính của robot, để các điểm còn lại mới thực sự là vùng mà robot có thể di chuyển.
Trong move_base, nội dung về bản đồ phình to không nhiều, chỉ có vài hàm đơn giản. Hãy cùng xem xét chúng.
1、computeCaches
Đây là một hàm tiền xử lý, vì vậy chúng ta sẽ xem xét nó đầu tiên. Hàm này thực hiện chức năng tính toán trước khoảng cách từ các điểm xung quanh chướng ngại vật đến chướng ngại vật và mối quan hệ giá trị chi phí, sau đó sẽ sử dụng phương pháp tra bảng để lấy dữ liệu cụ thể:
void InflationLayer::computeCaches()
{ //Trường hợp 1: Nếu bán kính phình to ô lưới là 0, thì trả về ngay, không cần làm gì cả.
if (cell_inflation_radius_ == 0)
return;
// dựa trên bán kính phình to... tính toán bộ nhớ đệm khoảng cách và chi phí
//Trường hợp 2: Nếu bán kính phình to ô lưới không bằng bán kính phình to ô lưới đã lưu trong cache, thì: dựa trên bán kính phình to... tính toán bộ nhớ đệm khoảng cách và chi phí
if (cell_inflation_radius_ != cached_cell_inflation_radius_)
{
deleteKernels();
cached_costs_ = new unsigned char*[cell_inflation_radius_ + 2]; //cấp phát lại bộ nhớ, tạo cached_costs_
cached_distances_ = new double*[cell_inflation_radius_ + 2]; //cấp phát lại bộ nhớ, tạo cached_distances_
//tạo hai vòng lặp lồng nhau, từ 0 đến bán kính phình to ô lưới + 1, lần lượt điền giá trị chi phí và khoảng cách
for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
{
cached_costs_[i] = new unsigned char[cell_inflation_radius_ + 2];
cached_distances_[i] = new double[cell_inflation_radius_ + 2];
for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j)
{//Hàm hypot là một hàm thư viện chuẩn C++, nó tính căn bậc hai của tổng bình phương hai số thực, tức là tính độ dài cạnh huyền của tam giác vuông.
cached_distances_[i][j] = hypot(i, j);
}
}
//cập nhật bán kính phình to đã lưu trong cache
cached_cell_inflation_radius_ = cell_inflation_radius_;
}
//tính toán giá trị chi phí
for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
{
for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j)
{
cached_costs_[i][j] = computeCost(cached_distances_[i][j]);
}
}
}
Ở đây, cell_inflation_radius_ chính là bán kính của robot. Khi bán kính của robot không bằng 0, cần tính một bảng phình to cached_costs_, chức năng của nó giống như hình dưới:
Hàm computeCost tính giá trị chi phí của một điểm, cài đặt cụ thể như sau:
/** @brief Cho một khoảng cách, tính toán một chi phí.
* @param distance Khoảng cách từ một chướng ngại vật tính bằng ô lưới
* @return Một giá trị chi phí cho khoảng cách */
//Đầu tiên, kiểm tra khoảng cách, nếu khoảng cách bằng 0, thì trả về giá trị chi phí của chướng ngại vật chết người LETHAL_OBSTACLE;
//Tiếp tục kiểm tra, nếu khoảng cách nhân với độ phân giải có nhỏ hơn bán kính nội tiếp không, nếu có, thì trả về giá trị chi phí của chướng ngại vật phình to trong bán kính nội tiếp của robot INSCRIBED_INFLATED_OBSTACLE;
//Nếu không, thì giá trị chi phí giảm dần theo khoảng cách Euclid, khoảng cách Euclid là khoảng cách độ phân giải, định nghĩa factor là hàm mũ, cuối cùng tính ra giá trị cost;
virtual inline unsigned char computeCost(double distance) const
{
unsigned char cost = 0;
if (distance == 0)
cost = LETHAL_OBSTACLE;
else if (distance * resolution_ <= inscribed_radius_)
cost = INSCRIBED_INFLATED_OBSTACLE;
else
{
// đảm bảo giá trị chi phí giảm theo khoảng cách Euclid
double euclidean_distance = distance * resolution_;
double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_));
cost = (unsigned char)((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
}
return cost;
}
Mã này phù hợp với hình ảnh trên. Đối với các điểm chướng ngại vật, giá trị của nó sẽ là 254, và các vùng có khoảng cách nhỏ hơn bán kính robot so với chướng ngại vật sẽ được gán giá trị 253, các điểm xa hơn sẽ có giá trị giảm theo hàm mũ.
2、updateCosts
Đây là hàm xử lý chính trong bản đồ phình to. Bản đồ được chuyển từ bản đồ chướng ngại vật thành bản đồ phình to chính là thông qua hàm này. Đầu tiên, cần lấy một bản đồ hiện có và thông tin kích thước của bản đồ:
unsigned char* master_array = master_grid.getCharMap(); //lấy bản đồ
unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY(); //lấy tọa độ x, y của lưới
Sau khi lấy được bản đồ, thuật toán sẽ duyệt qua bản đồ này và ghi lại tất cả các điểm chướng ngại vật:
std::vector<CellData>& obs_bin = inflation_cells_[0.0];
for (int j = min_j; j < max_j; j++)
{
for (int i = min_i; i < max_i; i++)
{
int index = master_grid.getIndex(i, j); //1) Lấy chỉ số index tương ứng với (i,j) từ master_grid
unsigned char cost = master_array[index]; //2) Lấy giá trị chi phí cost tương ứng với chỉ số từ master_grid
if (cost == LETHAL_OBSTACLE) //3) Đầu tiên, kiểm tra xem giá trị chi phí có bằng giá trị chi phí của chướng ngại vật chết người không, nếu có, thì đẩy vào
{ //Lưu ý rằng obs_bin ở đây là một con trỏ, dữ liệu thực sự được lưu trong inflation_cells_
//std::map<double, std::vector<CellData> > inflation_cells_;
obs_bin.push_back(CellData(index, i, j, i, j));
}
}
}
Lưu ý rằng các điểm được ghi lại đều được lưu trong inflation_cells_, đây là một kiểu dữ liệu std::map.
Bây giờ chúng ta đã biết tất cả các điểm chướng ngại vật, bước tiếp theo tự nhiên là cần phải mở rộng chúng. Việc mở rộng ở đây được thực hiện bằng hai vòng lặp for:
//Dựa trên hàng đợi phình to, thực hiện phình to
//Sau các thao tác trên, giờ đây inflation_queue_ chứa toàn bộ các ô chướng ngại vật, các ô này được đóng gói thành kiểu CellData, bao gồm chỉ số của chính nó, chỉ số của chướng ngại vật gần nhất (tức là chính nó), và khoảng cách (0)
std::map<double, std::vector<CellData> >::iterator bin;
for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin)
{ //Duyệt qua từng điểm chướng ngại vật
//map ánh xạ cặp khóa-giá trị, khóa là khoảng cách đến chướng ngại vật gần nhất, giá trị là ô lưới
//second là "giá trị" trong cặp "khóa"-"giá trị", tức là số lần lặp là số lượng celldata
for (int i = 0; i < bin->second.size(); ++i)
{
// xử lý tất cả các ô ở khoảng cách dist_bin.first
//Ghi lại celldata đang lặp lại trong "khóa double" này
const CellData& cell = bin->second[i];
//Ghi lại chỉ số index của celldata này
unsigned int index = cell.index_;
// bỏ qua nếu đã được thăm
if (seen_[index])
{
continue;
}
seen_[index] = true;
//Lấy tọa độ của ô lưới này và tọa độ của chướng ngại vật gần nhất
unsigned int mx = cell.x_;
unsigned int my = cell.y_;
unsigned int sx = cell.src_x_;
unsigned int sy = cell.src_y_;
// gán giá trị chi phí liên quan đến khoảng cách từ chướng ngại vật đến ô lưới
//4) Cập nhật giá trị cost của các điểm trong hàng đợi phình to
//Gán giá trị cost dựa trên khoảng cách từ CELL đến chướng ngại vật
unsigned char cost = costLookup(mx, my, sx, sy);
unsigned char old_cost = master_array[index]; //Tìm giá trị chi phí của chỉ số index đã chỉ định từ master_grid
//Nếu old_cost không có thông tin hoặc cost lớn hơn FREE_SPACE, cập nhật cost vào master_array
if (old_cost == NO_INFORMATION && (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE)))
master_array[index] = cost;
else//ngược lại, so sánh giá trị cũ và mới, cập nhật cost lớn hơn vào master_array
master_array[index] = std::max(old_cost, cost);
// thử đặt các ô lưới lân cận của ô lưới hiện tại vào danh sách phình to
//Trước tiên, lấy ô lưới có khoảng cách lớn nhất từ inflation_cells_, sau đó đẩy bốn ô lưới xung quanh (trên, dưới, trái, phải) vào inflation_cells_.
//Do hai tham số cuối cùng (sx, sy) không thay đổi khi gọi enqueue, điều này đảm bảo chỉ số này chắc chắn là ô chướng ngại vật.
//Vì ở入口 của enqueue sẽ kiểm tra if (!seen_[index]), điều này đảm bảo các ô lưới này không bị đẩy vào lặp lại.
//Vì đây là hàng đợi ưu tiên, do đó các điểm xung quanh chướng ngại vật, mỗi lần đều là điểm xa nhất so với chướng ngại vật bị lấy ra và được kiểm tra, điều này đảm bảo quá trình mở rộng này diễn ra theo hướng xa hơn so với chướng ngại vật.
//Thử đặt các ô lưới lân cận của ô lưới hiện tại vào hàng đợi
if (mx > 0)
enqueue(index - 1, mx - 1, my, sx, sy);
if (my > 0)
enqueue(index - size_x, mx, my - 1, sx, sy);
if (mx < size_x - 1)
enqueue(index + 1, mx + 1, my, sx, sy);
if (my < size_y - 1)
enqueue(index + size_x, mx, my + 1, sx, sy);
}
}
inflation_cells_.clear();
Thực tế, ở đây đang thực hiện một quá trình duyệt qua các điểm có thể, nhưng khá thú vị là thông thường loại duyệt này thường sử dụng while, nhưng ở đây lại sử dụng một vòng lặp for cho container map.
Ý tưởng chính như sau:
- Lưu tất cả các điểm chướng ngại vật vào container map
- Duyệt qua container map, lấy từng điểm chướng ngại vật, duyệt qua bốn ô lưới lân cận, tính toán khoảng cách từ bốn ô lưới lân cận đến chướng ngại vật gần nhất và giá trị chi phí của chúng, và lưu vào container map
- Duyệt qua các điểm gần chướng ngại vật, duyệt qua bốn ô lưới lân cận, tìm nhóm điểm tiếp theo xa hơn so với điểm hiện tại, tính toán giá trị chi phí và lưu trữ
- Lặp lại bước 3 cho đến khi các điểm còn lại đều vượt quá ngưỡng khoảng cách so với chướng ngại vật, thoát khỏi vòng lặp for.
Lưu ý tại sao vòng lặp for có thể thực hiện chức năng này, liên quan chặt chẽ đến kiểu dữ liệu ở trên: kiểu dữ liệu std::map có dữ liệu đầu tiên là khoảng cách từ điểm đến chướng ngại vật, ban đầu các điểm được lưu trong map đều là các điểm chướng ngại vật, vì vậy khóa của chúng đều là 0, sau đó trong quá trình duyệt các điểm này sẽ tạo ra một loạt các ô lưới lân cận, khoảng cách từ các ô lưới này đến chướng ngại vật đều là giá trị của một ô lưới, vì vậy map sẽ tạo ra một khóa mới và một nhóm thông tin bản đồ mới, như vậy đã thực hiện được việc mở rộng và duyệt lặp của inflation_cells_. Khi tất cả các điểm gần chướng ngại vật đều được duyệt qua, sẽ không tạo ra khóa mới nữa, thì quá trình duyệt lặp của inflation_cells_ cũng kết thúc.
Ngoài ra, còn có một thứ khá quan trọng ở đây:
// bỏ qua nếu đã được thăm
if (seen_[index])
{
continue;
}
seen_[index] = true;
Đối với mỗi điểm chỉ duyệt qua một lần, cách làm này là để ngăn chặn việc duyệt lặp lại gây ra các dữ liệu bất thường, vì đối với quá trình duyệt lặp ở trên, khoảng cách của điểm được duyệt qua ở thời điểm trước chắc chắn là gần hơn so với khoảng cách của điểm được duyệt qua ở vòng lặp tiếp theo (bắt đầu từ chướng ngại vật và mở rộng ra ngoài).
3、enqueue
Đây là một hàm chính khác trong bản đồ phình to, chức năng của nó là lan tỏa, tức là từ điểm chướng ngại vật dần dần lan tỏa ra ngoài cho đến khi tất cả các điểm gần chướng ngại vật đều được duyệt qua.
/**
* @brief Cho một chỉ số của ô lưới trong bản đồ chi phí, đặt nó vào danh sách chờ để phình to chướng ngại vật
* @param grid Bản đồ chi phí
* @param index Chỉ số của ô lưới
* @param mx Tọa độ x của ô lưới (có thể tính từ chỉ số, nhưng tiết kiệm thời gian nếu lưu trữ)
* @param my Tọa độ y của ô lưới (có thể tính từ chỉ số, nhưng tiết kiệm thời gian nếu lưu trữ)
* @param src_x Chỉ số x của điểm chướng ngại vật bắt đầu quá trình phình to
* @param src_y Chỉ số y của điểm chướng ngại vật bắt đầu quá trình phình to
*/
inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigned int my,
unsigned int src_x, unsigned int src_y)
{
if (!seen_[index])
{
// chúng ta tính bảng khoảng cách của mình một ô lưới xa hơn so với bán kính phình to quy định để có thể thực hiện kiểm tra dưới đây
//Tìm khoảng cách của nó đến chướng ngại vật gần nhất, nếu vượt quá ngưỡng, thì trả về ngay
//Nếu không vượt quá ngưỡng, thì cũng thêm nó vào mảng inflation_cells_
double distance = distanceLookup(mx, my, src_x, src_y);
// chúng ta chỉ muốn đặt ô lưới vào danh sách nếu nó nằm trong bán kính phình to của điểm chướng ngại vật
if (distance > cell_inflation_radius_)
return;
// đẩy dữ liệu ô lưới vào danh sách phình to và đánh dấu
inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y));
}
}
Trong updateCosts, đối với mỗi điểm, sẽ gọi hàm enqueue để duyệt qua bốn điểm xung quanh nó. Trong hàm enqueue, nếu điểm này đã được duyệt qua một lần thì bỏ qua, nếu không, sẽ dựa trên khoảng cách từ điểm này đến chướng ngại vật gần nhất có nhỏ hơn cell_inflation_radius_ hay không để quyết định có cần thực hiện tính toán updateCosts hay không. Đối với các điểm cần thiết sẽ được push vào inflation_cells_, tức là các điểm được thực hiện bởi hai vòng lặp for trong hàm thứ hai.
Lưu ý rằng cell_inflation_radius_ và inscribed_radius_ được sử dụng trong phần đầu tiên là hai tham số, tham số này nên lớn hơn một chút so với inscribed_radius_. Nếu hai tham số này được đặt bằng nhau, thì sẽ giống như trong hình phần đầu chỉ giữ lại các ô lưới có tham số 254 và 253, loại bỏ phần giảm theo hàm mũ.
Đến đây, bản đồ phình to cơ bản đã xem xong. Phần này ban đầu cảm thấy khá cao siêu, nhưng thực tế sau khi xem xong, thấy rằng nó không nhiều. Tóm lại, đó là tính toán trước một bảng tra cứu, trong bảng ghi lại giá trị chi phí của các điểm ở các khoảng cách khác nhau đến chướng ngại vật. Sau đó, duyệt qua từng điểm chướng ngại vật, các điểm chướng ngại vật này lan tỏa đồng thời ra ngoài, tính khoảng cách từ các điểm xung quanh đến chướng ngại vật, và dựa trên bảng tra cứu để tính giá trị chi phí của các ô lưới xung quanh. Cho đến khi tất cả các điểm đều lan tỏa đến các khoảng cách xa hơn so với chướng ngại vật, một bản đồ phình to được coi là đã hoàn thành.